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.
- robot_models/__init__.py +18 -0
- robot_models/actuators/__init__.py +1 -0
- robot_models/actuators/rungs/README.md +24 -0
- robot_models/actuators/rungs/coulomb_viscous/README.md +61 -0
- robot_models/actuators/rungs/coulomb_viscous/model.js +14 -0
- robot_models/actuators/rungs/coulomb_viscous/model.json +102 -0
- robot_models/actuators/rungs/coulomb_viscous/model.py +24 -0
- robot_models/actuators/rungs/datasheet/README.md +65 -0
- robot_models/actuators/rungs/datasheet/model.js +14 -0
- robot_models/actuators/rungs/datasheet/model.json +138 -0
- robot_models/actuators/rungs/datasheet/model.py +24 -0
- robot_models/actuators/rungs/dcmotor/README.md +63 -0
- robot_models/actuators/rungs/dcmotor/model.json +163 -0
- robot_models/actuators/rungs/ideal/README.md +60 -0
- robot_models/actuators/rungs/ideal/model.js +6 -0
- robot_models/actuators/rungs/ideal/model.json +90 -0
- robot_models/actuators/rungs/ideal/model.py +13 -0
- robot_models/actuators/rungs/load/README.md +65 -0
- robot_models/actuators/rungs/load/model.js +15 -0
- robot_models/actuators/rungs/load/model.json +117 -0
- robot_models/actuators/rungs/load/model.py +25 -0
- robot_models/actuators/rungs/lugre/README.md +72 -0
- robot_models/actuators/rungs/lugre/model.js +24 -0
- robot_models/actuators/rungs/lugre/model.json +164 -0
- robot_models/actuators/rungs/lugre/model.py +30 -0
- robot_models/actuators/rungs/ripple/README.md +64 -0
- robot_models/actuators/rungs/ripple/model.js +17 -0
- robot_models/actuators/rungs/ripple/model.json +119 -0
- robot_models/actuators/rungs/ripple/model.py +27 -0
- robot_models/actuators/rungs/stribeck/README.md +65 -0
- robot_models/actuators/rungs/stribeck/model.js +17 -0
- robot_models/actuators/rungs/stribeck/model.json +126 -0
- robot_models/actuators/rungs/stribeck/model.py +28 -0
- robot_models/actuators/servo/README.md +278 -0
- robot_models/actuators/servo/__init__.py +7 -0
- robot_models/actuators/servo/bldc/README.md +186 -0
- robot_models/actuators/servo/bldc/__init__.py +5 -0
- robot_models/actuators/servo/bldc/bldc_motor.py +93 -0
- robot_models/actuators/servo/bldc/ladder.py +268 -0
- robot_models/actuators/servo/foc_controller/README.md +127 -0
- robot_models/actuators/servo/foc_controller/__init__.py +3 -0
- robot_models/actuators/servo/foc_controller/ladder.py +207 -0
- robot_models/actuators/servo/gearbox/README.md +148 -0
- robot_models/actuators/servo/gearbox/__init__.py +5 -0
- robot_models/actuators/servo/gearbox/gearbox.py +36 -0
- robot_models/actuators/servo/gearbox/ladder.py +214 -0
- robot_models/actuators/servo/integrated_servo.py +33 -0
- robot_models/actuators/servo/ladder.py +593 -0
- robot_models/arms/__init__.py +1 -0
- robot_models/arms/rigid_body_chain/README.md +151 -0
- robot_models/arms/rigid_body_chain/__init__.py +3 -0
- robot_models/arms/rigid_body_chain/ladder.py +228 -0
- robot_models/conveyors/__init__.py +1 -0
- robot_models/conveyors/conveyor/README.md +140 -0
- robot_models/conveyors/conveyor/__init__.py +1 -0
- robot_models/conveyors/conveyor/ladder.py +203 -0
- robot_models/datasheet/__init__.py +56 -0
- robot_models/datasheet/datasheet.py +355 -0
- robot_models/datasheet/literal.py +144 -0
- robot_models/datasheet/source.py +328 -0
- robot_models/datasheet/stated.py +135 -0
- robot_models/datasheet/units.py +57 -0
- robot_models/end_effectors/__init__.py +1 -0
- robot_models/end_effectors/gripper/README.md +112 -0
- robot_models/end_effectors/gripper/__init__.py +3 -0
- robot_models/end_effectors/gripper/ladder.py +150 -0
- robot_models/homing/__init__.py +38 -0
- robot_models/homing/cad_bundle.py +206 -0
- robot_models/homing/contact.py +201 -0
- robot_models/homing/fit.py +126 -0
- robot_models/homing/metrics.py +237 -0
- robot_models/homing/protocols.py +72 -0
- robot_models/homing/types.py +256 -0
- robot_models/ladder.py +645 -0
- robot_models/markers/__init__.py +1 -0
- robot_models/markers/aruco_marker/README.md +92 -0
- robot_models/markers/aruco_marker/__init__.py +2 -0
- robot_models/markers/aruco_marker/ladder.py +142 -0
- robot_models/references.py +429 -0
- robot_models/sensors/__init__.py +1 -0
- robot_models/sensors/camera/README.md +118 -0
- robot_models/sensors/camera/__init__.py +3 -0
- robot_models/sensors/camera/ladder.py +108 -0
- robot_models/sensors/encoder/README.md +103 -0
- robot_models/sensors/encoder/__init__.py +5 -0
- robot_models/sensors/encoder/ladder.py +133 -0
- robot_models/sensors/hyperspectral_camera/README.md +107 -0
- robot_models/sensors/hyperspectral_camera/__init__.py +1 -0
- robot_models/sensors/hyperspectral_camera/ladder.py +151 -0
- robot_models/sensors/rgbd/README.md +113 -0
- robot_models/sensors/rgbd/__init__.py +5 -0
- robot_models/sensors/rgbd/ladder.py +113 -0
- robot_models_lib-0.2.0.dist-info/METADATA +143 -0
- robot_models_lib-0.2.0.dist-info/RECORD +96 -0
- robot_models_lib-0.2.0.dist-info/WHEEL +4 -0
- 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"]
|