robot-models-lib 0.2.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (96) hide show
  1. robot_models/__init__.py +18 -0
  2. robot_models/actuators/__init__.py +1 -0
  3. robot_models/actuators/rungs/README.md +24 -0
  4. robot_models/actuators/rungs/coulomb_viscous/README.md +61 -0
  5. robot_models/actuators/rungs/coulomb_viscous/model.js +14 -0
  6. robot_models/actuators/rungs/coulomb_viscous/model.json +102 -0
  7. robot_models/actuators/rungs/coulomb_viscous/model.py +24 -0
  8. robot_models/actuators/rungs/datasheet/README.md +65 -0
  9. robot_models/actuators/rungs/datasheet/model.js +14 -0
  10. robot_models/actuators/rungs/datasheet/model.json +138 -0
  11. robot_models/actuators/rungs/datasheet/model.py +24 -0
  12. robot_models/actuators/rungs/dcmotor/README.md +63 -0
  13. robot_models/actuators/rungs/dcmotor/model.json +163 -0
  14. robot_models/actuators/rungs/ideal/README.md +60 -0
  15. robot_models/actuators/rungs/ideal/model.js +6 -0
  16. robot_models/actuators/rungs/ideal/model.json +90 -0
  17. robot_models/actuators/rungs/ideal/model.py +13 -0
  18. robot_models/actuators/rungs/load/README.md +65 -0
  19. robot_models/actuators/rungs/load/model.js +15 -0
  20. robot_models/actuators/rungs/load/model.json +117 -0
  21. robot_models/actuators/rungs/load/model.py +25 -0
  22. robot_models/actuators/rungs/lugre/README.md +72 -0
  23. robot_models/actuators/rungs/lugre/model.js +24 -0
  24. robot_models/actuators/rungs/lugre/model.json +164 -0
  25. robot_models/actuators/rungs/lugre/model.py +30 -0
  26. robot_models/actuators/rungs/ripple/README.md +64 -0
  27. robot_models/actuators/rungs/ripple/model.js +17 -0
  28. robot_models/actuators/rungs/ripple/model.json +119 -0
  29. robot_models/actuators/rungs/ripple/model.py +27 -0
  30. robot_models/actuators/rungs/stribeck/README.md +65 -0
  31. robot_models/actuators/rungs/stribeck/model.js +17 -0
  32. robot_models/actuators/rungs/stribeck/model.json +126 -0
  33. robot_models/actuators/rungs/stribeck/model.py +28 -0
  34. robot_models/actuators/servo/README.md +278 -0
  35. robot_models/actuators/servo/__init__.py +7 -0
  36. robot_models/actuators/servo/bldc/README.md +186 -0
  37. robot_models/actuators/servo/bldc/__init__.py +5 -0
  38. robot_models/actuators/servo/bldc/bldc_motor.py +93 -0
  39. robot_models/actuators/servo/bldc/ladder.py +268 -0
  40. robot_models/actuators/servo/foc_controller/README.md +127 -0
  41. robot_models/actuators/servo/foc_controller/__init__.py +3 -0
  42. robot_models/actuators/servo/foc_controller/ladder.py +207 -0
  43. robot_models/actuators/servo/gearbox/README.md +148 -0
  44. robot_models/actuators/servo/gearbox/__init__.py +5 -0
  45. robot_models/actuators/servo/gearbox/gearbox.py +36 -0
  46. robot_models/actuators/servo/gearbox/ladder.py +214 -0
  47. robot_models/actuators/servo/integrated_servo.py +33 -0
  48. robot_models/actuators/servo/ladder.py +593 -0
  49. robot_models/arms/__init__.py +1 -0
  50. robot_models/arms/rigid_body_chain/README.md +151 -0
  51. robot_models/arms/rigid_body_chain/__init__.py +3 -0
  52. robot_models/arms/rigid_body_chain/ladder.py +228 -0
  53. robot_models/conveyors/__init__.py +1 -0
  54. robot_models/conveyors/conveyor/README.md +140 -0
  55. robot_models/conveyors/conveyor/__init__.py +1 -0
  56. robot_models/conveyors/conveyor/ladder.py +203 -0
  57. robot_models/datasheet/__init__.py +56 -0
  58. robot_models/datasheet/datasheet.py +355 -0
  59. robot_models/datasheet/literal.py +144 -0
  60. robot_models/datasheet/source.py +328 -0
  61. robot_models/datasheet/stated.py +135 -0
  62. robot_models/datasheet/units.py +57 -0
  63. robot_models/end_effectors/__init__.py +1 -0
  64. robot_models/end_effectors/gripper/README.md +112 -0
  65. robot_models/end_effectors/gripper/__init__.py +3 -0
  66. robot_models/end_effectors/gripper/ladder.py +150 -0
  67. robot_models/homing/__init__.py +38 -0
  68. robot_models/homing/cad_bundle.py +206 -0
  69. robot_models/homing/contact.py +201 -0
  70. robot_models/homing/fit.py +126 -0
  71. robot_models/homing/metrics.py +237 -0
  72. robot_models/homing/protocols.py +72 -0
  73. robot_models/homing/types.py +256 -0
  74. robot_models/ladder.py +645 -0
  75. robot_models/markers/__init__.py +1 -0
  76. robot_models/markers/aruco_marker/README.md +92 -0
  77. robot_models/markers/aruco_marker/__init__.py +2 -0
  78. robot_models/markers/aruco_marker/ladder.py +142 -0
  79. robot_models/references.py +429 -0
  80. robot_models/sensors/__init__.py +1 -0
  81. robot_models/sensors/camera/README.md +118 -0
  82. robot_models/sensors/camera/__init__.py +3 -0
  83. robot_models/sensors/camera/ladder.py +108 -0
  84. robot_models/sensors/encoder/README.md +103 -0
  85. robot_models/sensors/encoder/__init__.py +5 -0
  86. robot_models/sensors/encoder/ladder.py +133 -0
  87. robot_models/sensors/hyperspectral_camera/README.md +107 -0
  88. robot_models/sensors/hyperspectral_camera/__init__.py +1 -0
  89. robot_models/sensors/hyperspectral_camera/ladder.py +151 -0
  90. robot_models/sensors/rgbd/README.md +113 -0
  91. robot_models/sensors/rgbd/__init__.py +5 -0
  92. robot_models/sensors/rgbd/ladder.py +113 -0
  93. robot_models_lib-0.2.0.dist-info/METADATA +143 -0
  94. robot_models_lib-0.2.0.dist-info/RECORD +96 -0
  95. robot_models_lib-0.2.0.dist-info/WHEEL +4 -0
  96. robot_models_lib-0.2.0.dist-info/licenses/LICENSE +21 -0
@@ -0,0 +1,112 @@
1
+ > **Generated file.** Written by [`tools/make_type_docs.py`](../../../../tools/make_type_docs.py) from this folder's [`ladder.py`](ladder.py): edit the ladder, not this file, then run `python tools/make_type_docs.py` from the repository root.
2
+
3
+ # gripper
4
+
5
+ An end effector that grasps, on a chain's flange.
6
+
7
+ **6 rungs** · 5 parameters · 1 aliases · 0 packed names · 0 deployment-only names · folder `end_effectors/gripper/`.
8
+
9
+ In code: `ladders()["gripper"]` from [`robot_models.ladder`](../../ladder.py). Every type: [docs/TYPES.md](../../../../docs/TYPES.md). Spec: [PEP 0004](../../../../docs/peps/0004-model-ladders-by-type.md).
10
+ Projects the notes below name, and what they are: [MuJoCo](../../../../docs/TYPES.md#name-mujoco).
11
+
12
+ ## About
13
+
14
+ From the docstring of [`ladder.py`](ladder.py):
15
+
16
+ Spec: PEP 0004 §5.11 and §6.10.
17
+
18
+ The climb: M0 places a rigid tool at the tool centre point; M1 maps the width command to the finger opening; M2 adds
19
+ the grip force and the contact friction that holds an object; M2.wear lets that friction change with grasp cycles. M6
20
+ and M7 are the learned rungs every type has.
21
+
22
+ ## The ladder
23
+
24
+ ```mermaid
25
+ flowchart BT
26
+ M0_tcp["M0.tcp<br/>A rigid gripper at the<br/>tool centre point"]
27
+ M1_width_map["M1.width_map<br/>Finger width map"]
28
+ M2_contact["M2.contact<br/>Grip force and contact<br/>friction"]
29
+ M2_wear["M2.wear<br/>Finger pads that glaze<br/>with grasps"]
30
+ M6_residual{{"M6.residual<br/>Grey-box residual on<br/>M2.wear"}}
31
+ M7_learned(["M7.learned<br/>Black-box network"])
32
+ M0_tcp --> M1_width_map
33
+ M1_width_map --> M2_contact
34
+ M2_contact --> M2_wear
35
+ M2_wear --> M6_residual
36
+ M6_residual ~~~ M7_learned
37
+ classDef physics fill:#e8f1fb,stroke:#2f6aa3,color:#10243a
38
+ classDef wear fill:#e8f1fb,stroke:#2f6aa3,color:#10243a,stroke-dasharray:6 3
39
+ classDef residual fill:#fdf0dc,stroke:#b06a00,color:#3a2400
40
+ classDef learned fill:#f1e6fb,stroke:#7a3db3,color:#2a1040
41
+ class M0_tcp,M1_width_map,M2_contact physics
42
+ class M2_wear wear
43
+ class M6_residual residual
44
+ class M7_learned learned
45
+ ```
46
+
47
+ Read it bottom up: each solid arrow runs from a rung's base to the rung that extends it, and a rung needs everything below it on its own chain (its fallback). Blue box: a physics rung; dashed blue box: a wear rung (a parameter that grows with use); amber hexagon: the grey-box residual (M6) on a physics rung; purple rounded box: the learned net (M7), which extends no model.
48
+
49
+ ## Rungs
50
+
51
+ Bottom up. *Needs* is what the rung's model cannot run without, new over its base; *reads when stated* is what it uses when a source states it. A rung needs everything its base needs.
52
+
53
+ | rung | title | base | needs (adds) | reads when stated (optional) | code | equation | references |
54
+ | --- | --- | --- | --- | --- | --- | --- | --- |
55
+ | <a id="rung-m0-tcp"></a>`M0.tcp` | A rigid gripper at the tool centre point | — | `tcp_offset` | — | — | `tool = flange · tcp_offset` | [SICILIANO_2009](#ref-siciliano_2009) |
56
+ | <a id="rung-m1-width_map"></a>`M1.width_map` | Finger width map | [`M0.tcp`](#rung-m0-tcp) | `finger_width_map` | — | — | `width = finger_width_map(command)` | [PRATTICHIZZO_2016](#ref-prattichizzo_2016) |
57
+ | <a id="rung-m2-contact"></a>`M2.contact` | Grip force and contact friction | [`M1.width_map`](#rung-m1-width_map) | `grip_force_map`, `contact_friction` | — | — | `force = grip_force_map(command, width), each pad's normal force; the object holds while, at each contact, \|tangential\| ≤ contact_friction·force (two pads hold about twice one pad's)` | [PRATTICHIZZO_2016](#ref-prattichizzo_2016), [MUJOCO_CONTACT](#ref-mujoco_contact) |
58
+ | <a id="rung-m2-wear"></a>`M2.wear` | Finger pads that glaze with grasps<br>wears `contact_friction`: see [Wear](#wear) | [`M2.contact`](#rung-m2-contact) | `contact_friction_change` | — | — | `contact_friction(n) = contact_friction + contact_friction_change·n, n the grasp cycles so far` | [ARCHARD_1953](#ref-archard_1953), [LU_MEEKER_1993](#ref-lu_meeker_1993) |
59
+ | <a id="rung-m6-residual"></a>`M6.residual` | Grey-box residual on M2.wear | [`M2.wear`](#rung-m2-wear) | — | — | — | `M2.wear + an MLP residual width from 3 samples of width command, measured width and finger current` | [NGUYEN_TUONG_2010](#ref-nguyen_tuong_2010) |
60
+ | <a id="rung-m7-learned"></a>`M7.learned` | Black-box network | — | — | — | — | `an MLP over 5 samples of width command, width and finger current → finger force` | [HWANGBO_2019](#ref-hwangbo_2019) |
61
+
62
+ ## Wear
63
+
64
+ Which parameter grows with use, with what, by which law, at which rate.
65
+
66
+ | rung | parameter that wears | grows with | law | rate |
67
+ | --- | --- | --- | --- | --- |
68
+ | [`M2.wear`](#rung-m2-wear) | [`contact_friction`](#param-contact_friction) | grasp cycles (cycle) | linear: `p(u) = p0 + k·u` | [`contact_friction_change`](#param-contact_friction_change) |
69
+
70
+ ## Vocabulary
71
+
72
+ Every parameter the ladder holds, by the rung that first reads it. — as a unit: dimensionless (a ratio or a count).
73
+
74
+ | parameter | SI unit | shape | definition | first read on |
75
+ | --- | --- | --- | --- | --- |
76
+ | <a id="param-tcp_offset"></a>`tcp_offset` | m | pose | The tool centre point's offset from the flange the gripper mounts on. | [`M0.tcp`](#rung-m0-tcp) |
77
+ | <a id="param-finger_width_map"></a>`finger_width_map` | m | map | Finger opening as a function of the width command. | [`M1.width_map`](#rung-m1-width_map) |
78
+ | <a id="param-grip_force_map"></a>`grip_force_map` | N | map | The normal force each finger pad presses the object with, as a function of the command and the width: what a gauge squeezed between two jaws reads. A datasheet's gripping force summed over the jaws is twice it on a two-jaw gripper. | [`M2.contact`](#rung-m2-contact) |
79
+ | <a id="param-contact_friction"></a>`contact_friction` | — | scalar | Coefficient of friction between the finger pads and the object. | [`M2.contact`](#rung-m2-contact) |
80
+ | <a id="param-contact_friction_change"></a>`contact_friction_change` | 1/cycle | scalar | How much contact_friction changes per grasp cycle (pad wear and glazing). | [`M2.wear`](#rung-m2-wear) |
81
+
82
+ ## Aliases
83
+
84
+ Other names a source uses for one of these parameters. A note says how it differs when it is not the same quantity one for one. The unit is the one the source writes its values in; *(canonical)* after it marks an alias stored with no unit of its own, which robot_models reads as the canonical parameter's SI unit, shown here spelled out. A note can still qualify that unit.
85
+
86
+ | alias | canonical | unit | note |
87
+ | --- | --- | --- | --- |
88
+ | `contact_friction_change_per_cycle` | [`contact_friction_change`](#param-contact_friction_change) | 1/cycle *(canonical)* | |
89
+
90
+ ## Packed names
91
+
92
+ Names for one array that carries several of these parameters; never read as one parameter.
93
+
94
+ None.
95
+
96
+ ## Deployment-only names
97
+
98
+ Names some model reads that are not the type's own: a deployment sets them, or the mount gives them.
99
+
100
+ None.
101
+
102
+ ## References
103
+
104
+ The works the rungs cite, by their names in [`robot_models.references`](../../references.py).
105
+
106
+ - <a id="ref-archard_1953"></a>**ARCHARD_1953**: Archard (1953). Contact and rubbing of flat surfaces. Journal of Applied Physics. [doi:10.1063/1.1721448](https://doi.org/10.1063/1.1721448). What the rung uses: wear volume grows with the load carried times the distance slid. Cited by [`M2.wear`](#rung-m2-wear).
107
+ - <a id="ref-hwangbo_2019"></a>**HWANGBO_2019**: Hwangbo, Lee, Dosovitskiy, Bellicoso, Tsounis, Koltun, Hutter (2019). Learning agile and dynamic motor skills for legged robots. Science Robotics. [doi:10.1126/scirobotics.aau5872](https://doi.org/10.1126/scirobotics.aau5872). What the rung uses: ANYmal's actuator net: an MLP over position error and velocity at t, t − 10 ms, t − 20 ms, 3 × 32 softsign (arXiv:1901.08652). Cited by [`M7.learned`](#rung-m7-learned).
108
+ - <a id="ref-lu_meeker_1993"></a>**LU_MEEKER_1993**: Lu, Meeker (1993). Using degradation measures to estimate a time-to-failure distribution. Technometrics. [doi:10.1080/00401706.1993.10485038](https://doi.org/10.1080/00401706.1993.10485038). What the rung uses: a degradation path in usage, linear the simplest, crossing a failure threshold. Cited by [`M2.wear`](#rung-m2-wear).
109
+ - <a id="ref-mujoco_contact"></a>**MUJOCO_CONTACT**: MuJoCo (2026). Computation: the contact model. [https://mujoco.readthedocs.io/en/stable/computation/index.html](https://mujoco.readthedocs.io/en/stable/computation/index.html) (doc). What the rung uses: soft contact with friction cones, what the twin's grasp runs on. Cited by [`M2.contact`](#rung-m2-contact).
110
+ - <a id="ref-nguyen_tuong_2010"></a>**NGUYEN_TUONG_2010**: Nguyen-Tuong, Peters (2010). Using model knowledge for learning inverse dynamics. IEEE International Conference on Robotics and Automation. [doi:10.1109/ROBOT.2010.5509858](https://doi.org/10.1109/ROBOT.2010.5509858). What the rung uses: a physics model plus a learned residual. Cited by [`M6.residual`](#rung-m6-residual).
111
+ - <a id="ref-prattichizzo_2016"></a>**PRATTICHIZZO_2016**: Prattichizzo, Trinkle (2016). Grasping. In Springer Handbook of Robotics, 2nd ed. Springer. [doi:10.1007/978-3-319-32552-1_38](https://doi.org/10.1007/978-3-319-32552-1_38) (chapter). What the rung uses: contact, friction cones and grasp force. Cited by [`M1.width_map`](#rung-m1-width_map), [`M2.contact`](#rung-m2-contact).
112
+ - <a id="ref-siciliano_2009"></a>**SICILIANO_2009**: Siciliano, Sciavicco, Villani, Oriolo (2009). Robotics: Modelling, Planning and Control. Springer. [doi:10.1007/978-1-84628-642-1](https://doi.org/10.1007/978-1-84628-642-1) (book). What the rung uses: rigid-body joint dynamics and reflected inertia. Cited by [`M0.tcp`](#rung-m0-tcp).
@@ -0,0 +1,3 @@
1
+ """A gripper: an end effector that grasps, mounted on the flange of a chain. Its ladder of models (PEP 0004) runs from
2
+ a rigid tool at the tool centre point, through the finger width map, the grip force and the contact friction, to the
3
+ finger pads that glaze with grasps."""
@@ -0,0 +1,150 @@
1
+ """A gripper: an end effector that grasps, on a chain's flange. Its ladder of models (PEP 0004).
2
+
3
+ Spec: PEP 0004 §5.11 and §6.10.
4
+
5
+ The climb: M0 places a rigid tool at the tool centre point; M1 maps the width command to the finger opening; M2 adds
6
+ the grip force and the contact friction that holds an object; M2.wear lets that friction change with grasp cycles. M6
7
+ and M7 are the learned rungs every type has.
8
+ """
9
+
10
+ from __future__ import annotations
11
+
12
+ from robot_models.ladder import Alias, Ladder, Parameter, Rung, Wear
13
+ from robot_models.references import (
14
+ ARCHARD_1953,
15
+ HWANGBO_2019,
16
+ LU_MEEKER_1993,
17
+ MUJOCO_CONTACT,
18
+ NGUYEN_TUONG_2010,
19
+ PRATTICHIZZO_2016,
20
+ SICILIANO_2009,
21
+ )
22
+
23
+ # --- the vocabulary, grouped by the rung that first reads it ---------------------------------------------------------
24
+
25
+ VOCABULARY = (
26
+ # M0.tcp
27
+ Parameter(
28
+ name="tcp_offset",
29
+ unit="m",
30
+ shape="pose",
31
+ definition="The tool centre point's offset from the flange the gripper mounts on.",
32
+ ),
33
+ # M1.width_map
34
+ Parameter(
35
+ name="finger_width_map",
36
+ unit="m",
37
+ shape="map",
38
+ definition="Finger opening as a function of the width command.",
39
+ ),
40
+ # M2.contact
41
+ # WHY each pad's normal force, not the jaws' sum: M2.contact's cone is per contact, so the force it reads is the
42
+ # one at each pad; a datasheet that sums the jaws' forces states twice it, and read as one pad's it would make
43
+ # the rung hold twice what it can.
44
+ Parameter(
45
+ name="grip_force_map",
46
+ unit="N",
47
+ shape="map",
48
+ definition=("The normal force each finger pad presses the object with, as a function of the command and the "
49
+ "width: what a gauge squeezed between two jaws reads. A datasheet's gripping force summed over "
50
+ "the jaws is twice it on a two-jaw gripper."),
51
+ ),
52
+ Parameter(
53
+ name="contact_friction",
54
+ unit="",
55
+ definition="Coefficient of friction between the finger pads and the object.",
56
+ ),
57
+ # M2.wear
58
+ Parameter(
59
+ name="contact_friction_change",
60
+ unit="1/cycle",
61
+ definition="How much contact_friction changes per grasp cycle (pad wear and glazing).",
62
+ ),
63
+ )
64
+
65
+ # --- the rungs, M0 upward --------------------------------------------------------------------------------------------
66
+
67
+ RUNGS = (
68
+ Rung(
69
+ level=0,
70
+ name="tcp",
71
+ title="A rigid gripper at the tool centre point",
72
+ base=None,
73
+ adds=("tcp_offset",),
74
+ equation="tool = flange · tcp_offset",
75
+ references=(SICILIANO_2009,),
76
+ ),
77
+ Rung(
78
+ level=1,
79
+ name="width_map",
80
+ title="Finger width map",
81
+ base="M0.tcp",
82
+ adds=("finger_width_map",),
83
+ equation="width = finger_width_map(command)",
84
+ references=(PRATTICHIZZO_2016,),
85
+ ),
86
+ Rung(
87
+ level=2,
88
+ name="contact",
89
+ title="Grip force and contact friction",
90
+ base="M1.width_map",
91
+ adds=("grip_force_map", "contact_friction"),
92
+ # WHY the Coulomb friction cone as the holding test: a pad grips while the tangential load stays inside it
93
+ # (PEP 0004 §5.11); MUJOCO_CONTACT is the simulator's own statement of the same cone. WHY per contact: each
94
+ # pad carries its own cone, so two pads hold about twice what one does.
95
+ equation=("force = grip_force_map(command, width), each pad's normal force; the object holds while, at "
96
+ "each contact, |tangential| ≤ contact_friction·force (two pads hold about twice one pad's)"),
97
+ references=(PRATTICHIZZO_2016, MUJOCO_CONTACT),
98
+ ),
99
+ Rung(
100
+ level=2,
101
+ name="wear",
102
+ title="Finger pads that glaze with grasps",
103
+ base="M2.contact",
104
+ adds=("contact_friction_change",),
105
+ wears=(
106
+ Wear(
107
+ parameter="contact_friction",
108
+ usage="grasp cycles",
109
+ usage_unit="cycle",
110
+ law="linear",
111
+ rate="contact_friction_change",
112
+ ),
113
+ ),
114
+ equation="contact_friction(n) = contact_friction + contact_friction_change·n, n the grasp cycles so far",
115
+ references=(ARCHARD_1953, LU_MEEKER_1993),
116
+ ),
117
+ Rung(
118
+ level=6,
119
+ name="residual",
120
+ title="Grey-box residual on M2.wear",
121
+ base="M2.wear",
122
+ kind="residual",
123
+ equation=("M2.wear + an MLP residual width from 3 samples of width command, measured width and finger "
124
+ "current"),
125
+ references=(NGUYEN_TUONG_2010,),
126
+ ),
127
+ Rung(
128
+ level=7,
129
+ name="learned",
130
+ title="Black-box network",
131
+ base=None,
132
+ kind="learned",
133
+ equation="an MLP over 5 samples of width command, width and finger current → finger force",
134
+ references=(HWANGBO_2019,),
135
+ ),
136
+ )
137
+
138
+ # --- the same quantities under other names ---------------------------------------------------------------------------
139
+
140
+ ALIASES = (
141
+ Alias(name="contact_friction_change_per_cycle", canonical="contact_friction_change"),
142
+ )
143
+
144
+ LADDER = Ladder(
145
+ type="gripper",
146
+ title="An end effector that grasps, on a chain's flange.",
147
+ vocabulary=VOCABULARY,
148
+ rungs=RUNGS,
149
+ aliases=ALIASES,
150
+ )
@@ -0,0 +1,38 @@
1
+ """Homing stop core: where the stops are, what a stall shows, and the pure fit.
2
+
3
+ Imports numpy and robot_msgs only. Exact mesh contact lives in ``robot_models.homing.contact``
4
+ and the cad2robot bundle reader in ``robot_models.homing.cad_bundle`` (both need the
5
+ ``contact`` extra: coal + trimesh).
6
+ """
7
+
8
+ from robot_models.homing.fit import fit
9
+ from robot_models.homing.types import (
10
+ BACKLASH_KINDS,
11
+ LOWER,
12
+ SIDES,
13
+ UPPER,
14
+ BacklashModel,
15
+ BacklashTable,
16
+ HomingResult,
17
+ JointBacklash,
18
+ JointZero,
19
+ StallEvidence,
20
+ StopMap,
21
+ StopSide,
22
+ )
23
+
24
+ __all__ = [
25
+ "BACKLASH_KINDS",
26
+ "LOWER",
27
+ "SIDES",
28
+ "UPPER",
29
+ "BacklashModel",
30
+ "BacklashTable",
31
+ "HomingResult",
32
+ "JointBacklash",
33
+ "JointZero",
34
+ "StallEvidence",
35
+ "StopMap",
36
+ "StopSide",
37
+ "fit",
38
+ ]
@@ -0,0 +1,206 @@
1
+ """Read a cad2robot bundle's hard stops and place the meshes they name (the ``contact`` extra).
2
+
3
+ ``hardstops.json`` (schema ``cad2robot_hardstops/1``) names, per joint side, the stop pair
4
+ ``files`` (moving, fixed) and their ``links``. The meshes are placed exactly as the exporter's
5
+ URDF places them at q = 0 (``hardstops.json["urdf"]``, never build's ``.cad2robot.urdf``); the
6
+ joint axis is the URDF ``<axis>`` turned into the world frame at the zero pose.
7
+
8
+ stdlib XML for the URDF; trimesh only to read a mesh file.
9
+ """
10
+
11
+ from __future__ import annotations
12
+
13
+ import hashlib
14
+ import json
15
+ import math
16
+ import re
17
+ import xml.etree.ElementTree as ET
18
+ from dataclasses import dataclass
19
+ from pathlib import Path
20
+
21
+ import numpy as np
22
+
23
+ from robot_models.homing.contact import CONTACT_M, INTERFERENCE_M, Mesh
24
+ from robot_models.homing.types import LOWER, UPPER, StopMap, StopSide
25
+
26
+ SCHEMA = "cad2robot_hardstops/1"
27
+ _URI = re.compile(r"^(?:file://)?(?:package://[^/]+/)?")
28
+ _SIDE_KEYS = ((LOWER, "lower"), (UPPER, "upper"))
29
+
30
+
31
+ def _floats(text: str | None, default: tuple[float, ...]) -> tuple[float, ...]:
32
+ return tuple(float(x) for x in text.split()) if text else default
33
+
34
+
35
+ def _rpy(roll: float, pitch: float, yaw: float) -> np.ndarray:
36
+ cr, sr = math.cos(roll), math.sin(roll)
37
+ cp, sp = math.cos(pitch), math.sin(pitch)
38
+ cy, sy = math.cos(yaw), math.sin(yaw)
39
+ return np.array(
40
+ [
41
+ [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
42
+ [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
43
+ [-sp, cp * sr, cp * cr],
44
+ ]
45
+ )
46
+
47
+
48
+ def _origin(element: ET.Element | None) -> np.ndarray:
49
+ pose = np.eye(4)
50
+ tag = element.find("origin") if element is not None else None
51
+ if tag is not None:
52
+ pose[:3, :3] = _rpy(*_floats(tag.get("rpy"), (0.0, 0.0, 0.0)))
53
+ pose[:3, 3] = _floats(tag.get("xyz"), (0.0, 0.0, 0.0))
54
+ return pose
55
+
56
+
57
+ @dataclass(frozen=True)
58
+ class CadStop:
59
+ """One stop side as the exporter found it, with what is needed to find it again."""
60
+
61
+ stop: StopSide
62
+ moving_file: str
63
+ fixed_file: str
64
+ moving_link: str
65
+ fixed_link: str
66
+ axis_point: tuple[float, float, float]
67
+ axis: tuple[float, float, float]
68
+ contact_m: float
69
+ kind: str
70
+
71
+
72
+ class CadBundle:
73
+ """One exported robot folder (the one holding ``hardstops.json``)."""
74
+
75
+ def __init__(self, root: str | Path):
76
+ self.root = Path(root)
77
+ path = self.root / "hardstops.json"
78
+ raw = path.read_bytes()
79
+ self.hardstops_sha256 = hashlib.sha256(raw).hexdigest()
80
+ self.hardstops = json.loads(raw)
81
+ if self.hardstops.get("schema") != SCHEMA:
82
+ raise ValueError(f"{path}: schema {self.hardstops.get('schema')!r}, expected {SCHEMA!r}")
83
+ self.urdf_path = self.root / self.hardstops["urdf"]
84
+ self._parse_urdf()
85
+ self._mesh_cache: dict[str, tuple[np.ndarray, np.ndarray]] = {}
86
+
87
+ # -- the URDF at the zero pose --------------------------------------------------------
88
+
89
+ def _parse_urdf(self) -> None:
90
+ root = ET.parse(self.urdf_path).getroot()
91
+ self.visuals: dict[str, list[tuple[str, np.ndarray, np.ndarray]]] = {}
92
+ for link in root.iter("link"):
93
+ items = []
94
+ for visual in link.findall("visual"):
95
+ mesh = visual.find("geometry/mesh")
96
+ if mesh is None or not mesh.get("filename"):
97
+ continue
98
+ rel = _URI.sub("", mesh.get("filename").strip())
99
+ items.append((rel, np.array(_floats(mesh.get("scale"), (1.0, 1.0, 1.0))), _origin(visual)))
100
+ self.visuals[link.get("name")] = items
101
+ self.joints = {}
102
+ for joint in root.iter("joint"):
103
+ axis_tag = joint.find("axis")
104
+ axis = np.array(_floats(axis_tag.get("xyz") if axis_tag is not None else None, (1.0, 0.0, 0.0)))
105
+ self.joints[joint.get("name")] = (
106
+ joint.find("parent").get("link"),
107
+ joint.find("child").get("link"),
108
+ _origin(joint),
109
+ axis / np.linalg.norm(axis),
110
+ )
111
+ children = {child for _, child, _, _ in self.joints.values()}
112
+ self.world = {name: np.eye(4) for name in self.visuals if name not in children}
113
+ pending = dict(self.joints)
114
+ while pending:
115
+ placed = [name for name, (parent, *_) in pending.items() if parent in self.world]
116
+ if not placed:
117
+ raise ValueError(f"{self.urdf_path}: joints with no path to the root: {sorted(pending)}")
118
+ for name in placed:
119
+ parent, child, origin, _ = pending.pop(name)
120
+ self.world[child] = self.world[parent] @ origin
121
+
122
+ def joint_axis(self, joint: str) -> tuple[np.ndarray, np.ndarray]:
123
+ """(point on the axis, unit axis), world frame at the zero pose."""
124
+ parent, _, origin, axis = self.joints[joint]
125
+ frame = self.world[parent] @ origin
126
+ return frame[:3, 3].copy(), frame[:3, :3] @ axis
127
+
128
+ # -- stops --------------------------------------------------------------------------
129
+
130
+ def stop_map(self) -> StopMap:
131
+ """Every side of every swept joint; a null side is ``q_rad=None``, never dropped."""
132
+ sides = []
133
+ source = f"{self.root.name} hardstops.json sha256 {self.hardstops_sha256[:12]}"
134
+ for joint, entry in self.hardstops["joints"].items():
135
+ for side, key in _SIDE_KEYS:
136
+ found = entry.get(key)
137
+ sides.append(
138
+ StopSide(
139
+ joint,
140
+ side,
141
+ None if not found else float(found["q_rad"]),
142
+ basis="cad",
143
+ source=source,
144
+ pair=tuple(found["pair"]) if found else (),
145
+ )
146
+ )
147
+ return StopMap(tuple(sides), source=source)
148
+
149
+ def cad_stops(self) -> tuple[CadStop, ...]:
150
+ """The stop sides that exist, with their mesh pair, axis and touching criterion."""
151
+ stops = self.stop_map()
152
+ out = []
153
+ for joint, entry in self.hardstops["joints"].items():
154
+ point, axis = self.joint_axis(joint)
155
+ for side, key in _SIDE_KEYS:
156
+ found = entry.get(key)
157
+ if not found:
158
+ continue
159
+ # WHY: the exporter's loop-joint sweep (kind own_frame / interference) records no
160
+ # distance and stops at overlap; the plain sweep stops at contact_mm (PEP 0001).
161
+ proximity = found.get("distance_mm") is not None
162
+ contact = float(self.hardstops.get("contact_mm", 0.05)) / 1000.0 if proximity else INTERFERENCE_M
163
+ out.append(
164
+ CadStop(
165
+ stop=stops.get(joint, side),
166
+ moving_file=found["files"][0],
167
+ fixed_file=found["files"][1],
168
+ moving_link=found["links"][0],
169
+ fixed_link=found["links"][1],
170
+ axis_point=tuple(float(x) for x in point),
171
+ axis=tuple(float(x) for x in axis),
172
+ contact_m=contact,
173
+ kind=found.get("kind", "contact"),
174
+ )
175
+ )
176
+ return tuple(out)
177
+
178
+ # -- meshes -------------------------------------------------------------------------
179
+
180
+ def _read(self, rel: str) -> tuple[np.ndarray, np.ndarray]:
181
+ if rel not in self._mesh_cache:
182
+ import trimesh # noqa: PLC0415 -- the contact extra
183
+
184
+ mesh = trimesh.load(str(self.urdf_path.parent / rel), force="mesh", process=False)
185
+ faces = np.asarray(mesh.faces, np.int64)
186
+ if len(faces) == 0:
187
+ raise ValueError(f"{rel}: no triangles")
188
+ self._mesh_cache[rel] = (np.asarray(mesh.vertices, float), faces)
189
+ return self._mesh_cache[rel]
190
+
191
+ def placed(self, link: str, rel: str) -> list[Mesh]:
192
+ """Every placement of mesh file ``rel`` on ``link``, in the world frame at q = 0."""
193
+ out = []
194
+ for file, scale, link_to_mesh in self.visuals.get(link, []):
195
+ if file != rel:
196
+ continue
197
+ vertices, faces = self._read(rel)
198
+ pose = self.world[link] @ link_to_mesh
199
+ out.append(Mesh((vertices * scale) @ pose[:3, :3].T + pose[:3, 3], faces, f"{link}:{rel}"))
200
+ return out
201
+
202
+ def stop_meshes(self, stop: CadStop) -> tuple[list[Mesh], list[Mesh]]:
203
+ return self.placed(stop.moving_link, stop.moving_file), self.placed(stop.fixed_link, stop.fixed_file)
204
+
205
+
206
+ __all__ = ["CONTACT_M", "INTERFERENCE_M", "CadBundle", "CadStop"]