mink 0.0.1__py3-none-any.whl → 0.0.2__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.
- mink/__init__.py +11 -2
- mink/configuration.py +45 -25
- mink/lie/base.py +2 -2
- mink/lie/so3.py +2 -0
- mink/limits/collision_avoidance_limit.py +31 -35
- mink/limits/configuration_limit.py +23 -30
- mink/limits/limit.py +27 -9
- mink/limits/velocity_limit.py +30 -23
- mink/tasks/__init__.py +2 -0
- mink/tasks/com_task.py +24 -9
- mink/tasks/damping_task.py +19 -0
- mink/tasks/frame_task.py +45 -13
- mink/tasks/posture_task.py +39 -9
- mink/tasks/task.py +61 -28
- mink/utils.py +15 -7
- {mink-0.0.1.dist-info → mink-0.0.2.dist-info}/METADATA +36 -9
- {mink-0.0.1.dist-info → mink-0.0.2.dist-info}/RECORD +18 -17
- {mink-0.0.1.dist-info → mink-0.0.2.dist-info}/WHEEL +0 -0
mink/__init__.py
CHANGED
@@ -25,7 +25,15 @@ from .limits import (
|
|
25
25
|
VelocityLimit,
|
26
26
|
)
|
27
27
|
from .solve_ik import build_ik, solve_ik
|
28
|
-
from .tasks import
|
28
|
+
from .tasks import (
|
29
|
+
ComTask,
|
30
|
+
DampingTask,
|
31
|
+
FrameTask,
|
32
|
+
Objective,
|
33
|
+
PostureTask,
|
34
|
+
TargetNotSet,
|
35
|
+
Task,
|
36
|
+
)
|
29
37
|
from .utils import (
|
30
38
|
custom_configuration_vector,
|
31
39
|
get_body_geom_ids,
|
@@ -34,13 +42,14 @@ from .utils import (
|
|
34
42
|
move_mocap_to_frame,
|
35
43
|
)
|
36
44
|
|
37
|
-
__version__ = "0.0.
|
45
|
+
__version__ = "0.0.2"
|
38
46
|
|
39
47
|
__all__ = (
|
40
48
|
"ComTask",
|
41
49
|
"Configuration",
|
42
50
|
"build_ik",
|
43
51
|
"solve_ik",
|
52
|
+
"DampingTask",
|
44
53
|
"FrameTask",
|
45
54
|
"PostureTask",
|
46
55
|
"Task",
|
mink/configuration.py
CHANGED
@@ -1,3 +1,14 @@
|
|
1
|
+
"""Configuration space of a robot model.
|
2
|
+
|
3
|
+
The :class:`Configuration` class bundles a MuJoCo `model <https://mujoco.readthedocs.io/en/latest/APIreference/APItypes.html#mjmodel>`__
|
4
|
+
and `data <https://mujoco.readthedocs.io/en/latest/APIreference/APItypes.html#mjdata>`__,
|
5
|
+
and enables easy access to kinematic quantities such as frame transforms and frame
|
6
|
+
Jacobians.
|
7
|
+
|
8
|
+
Frames are coordinate systems that can be attached to different elements of
|
9
|
+
the robot model. mink supports frames of type `body`, `geom` and `site`.
|
10
|
+
"""
|
11
|
+
|
1
12
|
from typing import Optional
|
2
13
|
|
3
14
|
import mujoco
|
@@ -9,14 +20,24 @@ from .lie import SE3, SO3
|
|
9
20
|
|
10
21
|
|
11
22
|
class Configuration:
|
12
|
-
"""
|
13
|
-
|
23
|
+
"""Encapsulates a model and data for convenient access to kinematic quantities.
|
24
|
+
|
25
|
+
This class provides convenient methods to access and update the kinematic quantities
|
26
|
+
of a robot model, such as frame transforms and Jacobians. It ensures that forward
|
27
|
+
kinematics is computed at each time step, allowing the user to query up-to-date
|
28
|
+
information about the robot's state.
|
29
|
+
|
30
|
+
In this context, a frame refers to a coordinate system that can be attached to
|
31
|
+
different elements of the robot model. Currently supported frames include
|
32
|
+
`body`, `geom` and `site`.
|
14
33
|
|
15
|
-
|
16
|
-
called, namely:
|
34
|
+
Key functionalities include:
|
17
35
|
|
18
|
-
|
19
|
-
|
36
|
+
- Running forward kinematics to update the state.
|
37
|
+
- Checking configuration limits.
|
38
|
+
- Computing Jacobians for different frames.
|
39
|
+
- Retrieving frame transforms relative to the world frame.
|
40
|
+
- Integrating velocities to update configurations.
|
20
41
|
"""
|
21
42
|
|
22
43
|
def __init__(
|
@@ -27,8 +48,8 @@ class Configuration:
|
|
27
48
|
"""Constructor.
|
28
49
|
|
29
50
|
Args:
|
30
|
-
model:
|
31
|
-
q:
|
51
|
+
model: Mujoco model.
|
52
|
+
q: Configuration to initialize from. If None, the configuration
|
32
53
|
is initialized to the reference configuration `qpos0`.
|
33
54
|
"""
|
34
55
|
self.model = model
|
@@ -38,15 +59,13 @@ class Configuration:
|
|
38
59
|
def update(self, q: Optional[np.ndarray] = None) -> None:
|
39
60
|
"""Run forward kinematics.
|
40
61
|
|
41
|
-
|
42
|
-
kinematics) is `mj_kinematics`. An extra call to `mj_comPos` is needed for
|
43
|
-
updated Jacobians.
|
44
|
-
|
45
|
-
Args:`
|
62
|
+
Args:
|
46
63
|
q: Optional configuration vector to override internal data.qpos with.
|
47
64
|
"""
|
48
65
|
if q is not None:
|
49
66
|
self.data.qpos = q
|
67
|
+
# The minimal function call required to get updated frame transforms is
|
68
|
+
# mj_kinematics. An extra call to mj_comPos is required for updated Jacobians.
|
50
69
|
mujoco.mj_kinematics(self.model, self.data)
|
51
70
|
mujoco.mj_comPos(self.model, self.data)
|
52
71
|
|
@@ -95,24 +114,27 @@ class Configuration:
|
|
95
114
|
)
|
96
115
|
else:
|
97
116
|
print(
|
98
|
-
f"Value {qval} at index {jnt} is
|
99
|
-
f"[{qmin}, {qmax}]"
|
117
|
+
f"Value {qval:.2f} at index {jnt} is outside of its limits: "
|
118
|
+
f"[{qmin:.2f}, {qmax:.2f}]"
|
100
119
|
)
|
101
120
|
|
102
121
|
def get_frame_jacobian(self, frame_name: str, frame_type: str) -> np.ndarray:
|
103
|
-
"""Compute the Jacobian matrix of a frame velocity.
|
122
|
+
r"""Compute the Jacobian matrix of a frame velocity.
|
104
123
|
|
105
|
-
Denoting our frame by B and the world frame by W
|
106
|
-
is related to the body velocity
|
124
|
+
Denoting our frame by :math:`B` and the world frame by :math:`W`, the
|
125
|
+
Jacobian matrix :math:`{}_B J_{WB}` is related to the body velocity
|
126
|
+
:math:`{}_B v_{WB}` by:
|
107
127
|
|
108
|
-
|
128
|
+
.. math::
|
129
|
+
|
130
|
+
{}_B v_{WB} = {}_B J_{WB} \dot{q}
|
109
131
|
|
110
132
|
Args:
|
111
133
|
frame_name: Name of the frame in the MJCF.
|
112
134
|
frame_type: Type of frame. Can be a geom, a body or a site.
|
113
135
|
|
114
136
|
Returns:
|
115
|
-
Jacobian
|
137
|
+
Jacobian :math:`{}_B J_{WB}` of the frame.
|
116
138
|
"""
|
117
139
|
if frame_type not in consts.SUPPORTED_FRAMES:
|
118
140
|
raise exceptions.UnsupportedFrame(frame_type, consts.SUPPORTED_FRAMES)
|
@@ -142,9 +164,7 @@ class Configuration:
|
|
142
164
|
return jac
|
143
165
|
|
144
166
|
def get_transform_frame_to_world(self, frame_name: str, frame_type: str) -> SE3:
|
145
|
-
"""Get the pose of a frame
|
146
|
-
|
147
|
-
Denoting our frame by B and the world frame by W, this function returns T_WB.
|
167
|
+
"""Get the pose of a frame at the current configuration.
|
148
168
|
|
149
169
|
Args:
|
150
170
|
frame_name: Name of the frame in the MJCF.
|
@@ -177,7 +197,7 @@ class Configuration:
|
|
177
197
|
"""Integrate a velocity starting from the current configuration.
|
178
198
|
|
179
199
|
Args:
|
180
|
-
velocity: The velocity.
|
200
|
+
velocity: The velocity in tangent space.
|
181
201
|
dt: Integration duration in [s].
|
182
202
|
|
183
203
|
Returns:
|
@@ -191,7 +211,7 @@ class Configuration:
|
|
191
211
|
"""Integrate a velocity and update the current configuration inplace.
|
192
212
|
|
193
213
|
Args:
|
194
|
-
velocity: The velocity.
|
214
|
+
velocity: The velocity in tangent space.
|
195
215
|
dt: Integration duration in [s].
|
196
216
|
"""
|
197
217
|
mujoco.mj_integratePos(self.model, self.data.qpos, velocity, dt)
|
mink/lie/base.py
CHANGED
@@ -10,8 +10,8 @@ class MatrixLieGroup(abc.ABC):
|
|
10
10
|
|
11
11
|
Attributes:
|
12
12
|
matrix_dim: Dimension of square matrix output.
|
13
|
-
parameters_dim
|
14
|
-
tangent_dim
|
13
|
+
parameters_dim: Dimension of underlying parameters.
|
14
|
+
tangent_dim: Dimension of tangent space.
|
15
15
|
space_dim: Dimension of coordinates that can be transformed.
|
16
16
|
"""
|
17
17
|
|
mink/lie/so3.py
CHANGED
@@ -1,32 +1,4 @@
|
|
1
|
-
"""Collision avoidance limit.
|
2
|
-
|
3
|
-
Derivation
|
4
|
-
==========
|
5
|
-
|
6
|
-
p1, p2: closest points between g1 and g2
|
7
|
-
d: distance between g1 and g2 (d = ||p1 - p2||)
|
8
|
-
n: normal vector from g1 to g2 (n = (p2 - p1) / ||p2 - p1||)
|
9
|
-
|
10
|
-
The relative velocity constraint between g1 and g2 is given by:
|
11
|
-
|
12
|
-
n^T [J2(p2) - J1(p1)] dq <= -k * (d - d_min) / dt
|
13
|
-
V_n <= -k * (d - d_min)
|
14
|
-
|
15
|
-
where [J2(p2) - J1(p1)] dq denotes the relative velocity between g1 and g2, and
|
16
|
-
n^T [J2(p2) - J1(p1)] dq projects this velocity onto the direction connecting the
|
17
|
-
closest points p1 and p2.
|
18
|
-
|
19
|
-
We have three cases for the distance d:
|
20
|
-
|
21
|
-
1. V_n <= R_b if dist_c < dist_m
|
22
|
-
2. V_n <= -k * (dist_c - dist_m) / dt + R_b if dist_m <= d <= dist_d
|
23
|
-
3. V_n <= inf otherwise
|
24
|
-
|
25
|
-
where:
|
26
|
-
- dist_c: current normal distance between g1 and g2
|
27
|
-
- dist_m: minimum allowed distance between g1 and g2
|
28
|
-
- dist_d: collision detection distance
|
29
|
-
"""
|
1
|
+
"""Collision avoidance limit."""
|
30
2
|
|
31
3
|
import itertools
|
32
4
|
from dataclasses import dataclass
|
@@ -46,7 +18,7 @@ CollisionPairs = Sequence[CollisionPair]
|
|
46
18
|
|
47
19
|
|
48
20
|
@dataclass(frozen=True)
|
49
|
-
class
|
21
|
+
class _Contact:
|
50
22
|
dist: float
|
51
23
|
fromto: np.ndarray
|
52
24
|
geom1: int
|
@@ -107,7 +79,31 @@ def _is_pass_contype_conaffinity_check(
|
|
107
79
|
|
108
80
|
|
109
81
|
class CollisionAvoidanceLimit(Limit):
|
110
|
-
"""Normal velocity limit between geom pairs.
|
82
|
+
"""Normal velocity limit between geom pairs.
|
83
|
+
|
84
|
+
Attributes:
|
85
|
+
model: MuJoCo model.
|
86
|
+
geom_pairs: Set of collision pairs in which to perform active collision
|
87
|
+
avoidance. A collision pair is defined as a pair of geom groups. A geom
|
88
|
+
group is a set of geom names. For each geom pair, the solver will
|
89
|
+
attempt to compute joint velocities that avoid collisions between every
|
90
|
+
geom in the first geom group with every geom in the second geom group.
|
91
|
+
Self collision is achieved by adding a collision pair with the same
|
92
|
+
geom group in both pair fields.
|
93
|
+
gain: Gain factor in (0, 1] that determines how fast the geoms are
|
94
|
+
allowed to move towards each other at each iteration. Smaller values
|
95
|
+
are safer but may make the geoms move slower towards each other.
|
96
|
+
minimum_distance_from_collisions: The minimum distance to leave between
|
97
|
+
any two geoms. A negative distance allows the geoms to penetrate by
|
98
|
+
the specified amount.
|
99
|
+
collision_detection_distance: The distance between two geoms at which the
|
100
|
+
active collision avoidance limit will be active. A large value will
|
101
|
+
cause collisions to be detected early, but may incur high computational
|
102
|
+
cost. A negative value will cause the geoms to be detected only after
|
103
|
+
they penetrate by the specified amount.
|
104
|
+
bound_relaxation: An offset on the upper bound of each collision avoidance
|
105
|
+
constraint.
|
106
|
+
"""
|
111
107
|
|
112
108
|
def __init__(
|
113
109
|
self,
|
@@ -141,7 +137,7 @@ class CollisionAvoidanceLimit(Limit):
|
|
141
137
|
cost. A negative value will cause the geoms to be detected only after
|
142
138
|
they penetrate by the specified amount.
|
143
139
|
bound_relaxation: An offset on the upper bound of each collision avoidance
|
144
|
-
constraint
|
140
|
+
constraint.
|
145
141
|
"""
|
146
142
|
self.model = model
|
147
143
|
self.gain = gain
|
@@ -178,7 +174,7 @@ class CollisionAvoidanceLimit(Limit):
|
|
178
174
|
|
179
175
|
def _compute_contact_with_minimum_distance(
|
180
176
|
self, data: mujoco.MjData, geom1_id: int, geom2_id: int
|
181
|
-
) ->
|
177
|
+
) -> _Contact:
|
182
178
|
"""Returns the smallest signed distance between a geom pair."""
|
183
179
|
fromto = np.empty(6)
|
184
180
|
dist = mujoco.mj_geomDistance(
|
@@ -189,12 +185,12 @@ class CollisionAvoidanceLimit(Limit):
|
|
189
185
|
self.collision_detection_distance,
|
190
186
|
fromto,
|
191
187
|
)
|
192
|
-
return
|
188
|
+
return _Contact(
|
193
189
|
dist, fromto, geom1_id, geom2_id, self.collision_detection_distance
|
194
190
|
)
|
195
191
|
|
196
192
|
def _compute_contact_normal_jacobian(
|
197
|
-
self, data: mujoco.MjData, contact:
|
193
|
+
self, data: mujoco.MjData, contact: _Contact
|
198
194
|
) -> np.ndarray:
|
199
195
|
"""Computes the Jacobian mapping joint velocities to the normal component of
|
200
196
|
the relative Cartesian linear velocity between the geom pair.
|
@@ -1,24 +1,4 @@
|
|
1
|
-
"""
|
2
|
-
|
3
|
-
Derivation
|
4
|
-
==========
|
5
|
-
|
6
|
-
Using a first order Taylor expansion on the configuration, we can write the limit as:
|
7
|
-
|
8
|
-
q_min <= q + v * dt <= q_max
|
9
|
-
q_min <= q + dq <= q_max
|
10
|
-
q_min - q <= dq <= q_max - q
|
11
|
-
|
12
|
-
Rewriting as G dq <= h:
|
13
|
-
|
14
|
-
+I * dq <= q_max - q
|
15
|
-
-I * dq <= q - q_min
|
16
|
-
|
17
|
-
Stacking them together, we get:
|
18
|
-
|
19
|
-
G = [+I, -I]
|
20
|
-
h = [q_max - q, q - q_min]
|
21
|
-
"""
|
1
|
+
"""Joint position limit."""
|
22
2
|
|
23
3
|
import mujoco
|
24
4
|
import numpy as np
|
@@ -30,16 +10,9 @@ from .limit import Constraint, Limit
|
|
30
10
|
|
31
11
|
|
32
12
|
class ConfigurationLimit(Limit):
|
33
|
-
"""
|
34
|
-
|
35
|
-
Floating base joints (joint type="free") are ignored.
|
13
|
+
"""Inequality constraint on joint positions in a robot model.
|
36
14
|
|
37
|
-
|
38
|
-
indices: Tangent indices corresponding to configuration-limited joints.
|
39
|
-
projection_matrix: Projection from tangent space to subspace with
|
40
|
-
configuration-limited joints.
|
41
|
-
lower: Lower configuration limit.
|
42
|
-
upper: Upper configuration limit.
|
15
|
+
Floating base joints are ignored.
|
43
16
|
"""
|
44
17
|
|
45
18
|
def __init__(
|
@@ -95,6 +68,26 @@ class ConfigurationLimit(Limit):
|
|
95
68
|
configuration: Configuration,
|
96
69
|
dt: float,
|
97
70
|
) -> Constraint:
|
71
|
+
r"""Compute the configuration-dependent joint position limits.
|
72
|
+
|
73
|
+
The limits are defined as:
|
74
|
+
|
75
|
+
.. math::
|
76
|
+
|
77
|
+
{q \ominus q_{min}} \leq \Delta q \leq {q_{max} \ominus q}
|
78
|
+
|
79
|
+
where :math:`q \in {\cal C}` is the robot's configuration and
|
80
|
+
:math:`\Delta q \in T_q({\cal C})` is the displacement in the tangent
|
81
|
+
space at :math:`q`. See the :ref:`derivations` section for more information.
|
82
|
+
|
83
|
+
Args:
|
84
|
+
configuration: Robot configuration :math:`q`.
|
85
|
+
dt: Integration timestep in [s].
|
86
|
+
|
87
|
+
Returns:
|
88
|
+
Pair :math:`(G, h)` representing the inequality constraint as
|
89
|
+
:math:`G \Delta q \leq h`, or ``None`` if there is no limit.
|
90
|
+
"""
|
98
91
|
del dt # Unused.
|
99
92
|
|
100
93
|
# Upper.
|
mink/limits/limit.py
CHANGED
@@ -1,3 +1,5 @@
|
|
1
|
+
"""All kinematic limits derive from the :class:`Limit` base class."""
|
2
|
+
|
1
3
|
import abc
|
2
4
|
from typing import NamedTuple, Optional
|
3
5
|
|
@@ -7,21 +9,29 @@ from ..configuration import Configuration
|
|
7
9
|
|
8
10
|
|
9
11
|
class Constraint(NamedTuple):
|
10
|
-
"""Linear
|
12
|
+
r"""Linear inequality constraint of the form :math:`G(q) \Delta q \leq h(q)`.
|
11
13
|
|
12
|
-
|
14
|
+
Inactive if G and h are None.
|
13
15
|
"""
|
14
16
|
|
15
|
-
G: Optional[np.ndarray] = None
|
16
|
-
|
17
|
+
G: Optional[np.ndarray] = None
|
18
|
+
"""Shape (nv, nv)."""
|
19
|
+
h: Optional[np.ndarray] = None
|
20
|
+
"""Shape (nv,)."""
|
17
21
|
|
18
22
|
@property
|
19
23
|
def inactive(self) -> bool:
|
20
|
-
|
24
|
+
"""Returns True if the constraint is inactive."""
|
25
|
+
return self.G is None and self.h is None
|
21
26
|
|
22
27
|
|
23
28
|
class Limit(abc.ABC):
|
24
|
-
"""Abstract base class for kinematic limits.
|
29
|
+
"""Abstract base class for kinematic limits.
|
30
|
+
|
31
|
+
Subclasses must implement the :py:meth:`~Limit.compute_qp_inequalities` method
|
32
|
+
which takes in the current robot configuration and integration time step and
|
33
|
+
returns an instance of :class:`Constraint`.
|
34
|
+
"""
|
25
35
|
|
26
36
|
@abc.abstractmethod
|
27
37
|
def compute_qp_inequalities(
|
@@ -29,13 +39,21 @@ class Limit(abc.ABC):
|
|
29
39
|
configuration: Configuration,
|
30
40
|
dt: float,
|
31
41
|
) -> Constraint:
|
32
|
-
"""Compute limit as linearized QP inequalities
|
42
|
+
r"""Compute limit as linearized QP inequalities of the form:
|
43
|
+
|
44
|
+
.. math::
|
45
|
+
|
46
|
+
G(q) \Delta q \leq h(q)
|
47
|
+
|
48
|
+
where :math:`q \in {\cal C}` is the robot's configuration and
|
49
|
+
:math:`\Delta q \in T_q({\cal C})` is the displacement in the tangent
|
50
|
+
space at :math:`q`.
|
33
51
|
|
34
52
|
Args:
|
35
|
-
configuration:
|
53
|
+
configuration: Robot configuration :math:`q`.
|
36
54
|
dt: Integration time step in [s].
|
37
55
|
|
38
56
|
Returns:
|
39
|
-
Pair (G, h)
|
57
|
+
Pair :math:`(G, h)`.
|
40
58
|
"""
|
41
59
|
raise NotImplementedError
|
mink/limits/velocity_limit.py
CHANGED
@@ -1,24 +1,4 @@
|
|
1
|
-
"""
|
2
|
-
|
3
|
-
Derivation
|
4
|
-
==========
|
5
|
-
|
6
|
-
Given maximum joint velocity magnitudes v_max, we can express joint velocity limits as:
|
7
|
-
|
8
|
-
-v_max <= v <= v_max
|
9
|
-
-v_max <= dq / dt <= v_max
|
10
|
-
-v_max * dt <= dq <= v_max * dt
|
11
|
-
|
12
|
-
Rewriting as G dq <= h:
|
13
|
-
|
14
|
-
+I * dq <= v_max * dt
|
15
|
-
-I * dq <= v_max * dt
|
16
|
-
|
17
|
-
Stacking them together, we get:
|
18
|
-
|
19
|
-
G = [+I, -I]
|
20
|
-
h = [v_max * dt, v_max * dt]
|
21
|
-
"""
|
1
|
+
"""Joint velocity limit."""
|
22
2
|
|
23
3
|
from typing import Mapping
|
24
4
|
|
@@ -33,17 +13,22 @@ from .limit import Constraint, Limit
|
|
33
13
|
|
34
14
|
|
35
15
|
class VelocityLimit(Limit):
|
36
|
-
"""
|
16
|
+
"""Inequality constraint on joint velocities in a robot model.
|
37
17
|
|
38
18
|
Floating base joints are ignored.
|
39
19
|
|
40
20
|
Attributes:
|
41
21
|
indices: Tangent indices corresponding to velocity-limited joints.
|
42
|
-
limit: Maximum allowed velocity magnitude for velocity-limited joints
|
22
|
+
limit: Maximum allowed velocity magnitude for velocity-limited joints, in
|
23
|
+
[m]/[s] for slide joints and [rad]/[s] for hinge joints.
|
43
24
|
projection_matrix: Projection from tangent space to subspace with
|
44
25
|
velocity-limited joints.
|
45
26
|
"""
|
46
27
|
|
28
|
+
indices: np.ndarray
|
29
|
+
limit: np.ndarray
|
30
|
+
projection_matrix: np.ndarray
|
31
|
+
|
47
32
|
def __init__(
|
48
33
|
self,
|
49
34
|
model: mujoco.MjModel,
|
@@ -52,6 +37,7 @@ class VelocityLimit(Limit):
|
|
52
37
|
"""Initialize velocity limits.
|
53
38
|
|
54
39
|
Args:
|
40
|
+
model: MuJoCo model.
|
55
41
|
velocities: Dictionary mapping joint name to maximum allowed magnitude in
|
56
42
|
[m]/[s] for slide joints and [rad]/[s] for hinge joints.
|
57
43
|
"""
|
@@ -84,6 +70,27 @@ class VelocityLimit(Limit):
|
|
84
70
|
def compute_qp_inequalities(
|
85
71
|
self, configuration: Configuration, dt: float
|
86
72
|
) -> Constraint:
|
73
|
+
r"""Compute the configuration-dependent joint velocity limits.
|
74
|
+
|
75
|
+
The limits are defined as:
|
76
|
+
|
77
|
+
.. math::
|
78
|
+
|
79
|
+
-v_{\text{max}} \cdot dt \leq \Delta q \leq v_{\text{max}} \cdot dt
|
80
|
+
|
81
|
+
where :math:`v_{max} \in {\cal T}` is the robot's velocity limit
|
82
|
+
vector and :math:`\Delta q \in T_q({\cal C})` is the displacement in the
|
83
|
+
tangent space at :math:`q`. See the :ref:`derivations` section for
|
84
|
+
more information.
|
85
|
+
|
86
|
+
Args:
|
87
|
+
configuration: Robot configuration :math:`q`.
|
88
|
+
dt: Integration timestep in [s].
|
89
|
+
|
90
|
+
Returns:
|
91
|
+
Pair :math:`(G, h)` representing the inequality constraint as
|
92
|
+
:math:`G \Delta q \leq h`, or ``None`` if there is no limit.
|
93
|
+
"""
|
87
94
|
del configuration # Unused.
|
88
95
|
if self.projection_matrix is None:
|
89
96
|
return Constraint()
|
mink/tasks/__init__.py
CHANGED
@@ -1,6 +1,7 @@
|
|
1
1
|
"""Kinematic tasks."""
|
2
2
|
|
3
3
|
from .com_task import ComTask
|
4
|
+
from .damping_task import DampingTask
|
4
5
|
from .exceptions import (
|
5
6
|
InvalidDamping,
|
6
7
|
InvalidGain,
|
@@ -16,6 +17,7 @@ __all__ = (
|
|
16
17
|
"ComTask",
|
17
18
|
"FrameTask",
|
18
19
|
"Objective",
|
20
|
+
"DampingTask",
|
19
21
|
"PostureTask",
|
20
22
|
"Task",
|
21
23
|
"TargetNotSet",
|
mink/tasks/com_task.py
CHANGED
@@ -1,4 +1,6 @@
|
|
1
|
-
"""Center
|
1
|
+
"""Center-of-mass task implementation."""
|
2
|
+
|
3
|
+
from __future__ import annotations
|
2
4
|
|
3
5
|
from typing import Optional
|
4
6
|
|
@@ -12,7 +14,7 @@ from .task import Task
|
|
12
14
|
|
13
15
|
|
14
16
|
class ComTask(Task):
|
15
|
-
"""Regulate the center
|
17
|
+
"""Regulate the center-of-mass (CoM) of a robot.
|
16
18
|
|
17
19
|
Attributes:
|
18
20
|
target_com: Target position of the CoM.
|
@@ -33,7 +35,6 @@ class ComTask(Task):
|
|
33
35
|
self.set_cost(cost)
|
34
36
|
|
35
37
|
def set_cost(self, cost: npt.ArrayLike) -> None:
|
36
|
-
"""Set a new cost for all CoM coordinates."""
|
37
38
|
cost = np.atleast_1d(cost)
|
38
39
|
if cost.ndim != 1 or cost.shape[0] not in (1, self.k):
|
39
40
|
raise TaskDefinitionError(
|
@@ -46,7 +47,11 @@ class ComTask(Task):
|
|
46
47
|
self.cost[:] = cost
|
47
48
|
|
48
49
|
def set_target(self, target_com: npt.ArrayLike) -> None:
|
49
|
-
"""Set the target CoM position in the world frame.
|
50
|
+
"""Set the target CoM position in the world frame.
|
51
|
+
|
52
|
+
Args:
|
53
|
+
target_com: Desired center-of-mass position in the world frame.
|
54
|
+
"""
|
50
55
|
target_com = np.atleast_1d(target_com)
|
51
56
|
if target_com.ndim != 1 or target_com.shape[0] != (self.k):
|
52
57
|
raise InvalidTarget(
|
@@ -56,14 +61,21 @@ class ComTask(Task):
|
|
56
61
|
self.target_com = target_com.copy()
|
57
62
|
|
58
63
|
def set_target_from_configuration(self, configuration: Configuration) -> None:
|
59
|
-
"""Set the target CoM from a given robot configuration.
|
64
|
+
"""Set the target CoM from a given robot configuration.
|
65
|
+
|
66
|
+
Args:
|
67
|
+
configuration: Robot configuration :math:`q`.
|
68
|
+
"""
|
60
69
|
self.set_target(configuration.data.subtree_com[1])
|
61
70
|
|
62
71
|
def compute_error(self, configuration: Configuration) -> np.ndarray:
|
63
72
|
"""Compute the CoM task error.
|
64
73
|
|
65
|
-
|
66
|
-
|
74
|
+
Args:
|
75
|
+
configuration: Robot configuration :math:`q`.
|
76
|
+
|
77
|
+
Returns:
|
78
|
+
Center-of-mass task error vector :math:`e(q)`.
|
67
79
|
"""
|
68
80
|
if self.target_com is None:
|
69
81
|
raise TargetNotSet(self.__class__.__name__)
|
@@ -72,8 +84,11 @@ class ComTask(Task):
|
|
72
84
|
def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
|
73
85
|
"""Compute the CoM task Jacobian.
|
74
86
|
|
75
|
-
|
76
|
-
|
87
|
+
Args:
|
88
|
+
configuration: Robot configuration :math:`q`.
|
89
|
+
|
90
|
+
Returns:
|
91
|
+
Center-of-mass task jacobian :math:`J(q)`.
|
77
92
|
"""
|
78
93
|
if self.target_com is None:
|
79
94
|
raise TargetNotSet(self.__class__.__name__)
|
@@ -0,0 +1,19 @@
|
|
1
|
+
"""Damping task implementation."""
|
2
|
+
|
3
|
+
from __future__ import annotations
|
4
|
+
|
5
|
+
import mujoco
|
6
|
+
|
7
|
+
from .posture_task import PostureTask
|
8
|
+
|
9
|
+
|
10
|
+
class DampingTask(PostureTask):
|
11
|
+
"""Minimize joint velocities.
|
12
|
+
|
13
|
+
This task is implemented as a special case of the PostureTask where the gain and
|
14
|
+
target configuration are set to 0 and qpos0 respectively.
|
15
|
+
"""
|
16
|
+
|
17
|
+
def __init__(self, model: mujoco.MjModel, cost: float):
|
18
|
+
super().__init__(model=model, cost=cost, gain=0.0, lm_damping=0.0)
|
19
|
+
self.target_q = model.qpos0
|
mink/tasks/frame_task.py
CHANGED
@@ -1,5 +1,7 @@
|
|
1
1
|
"""Frame task implementation."""
|
2
2
|
|
3
|
+
from __future__ import annotations
|
4
|
+
|
3
5
|
from typing import Optional
|
4
6
|
|
5
7
|
import numpy as np
|
@@ -12,12 +14,13 @@ from .task import Task
|
|
12
14
|
|
13
15
|
|
14
16
|
class FrameTask(Task):
|
15
|
-
"""Regulate the pose of a
|
17
|
+
"""Regulate the pose of a frame expressed in the world frame.
|
16
18
|
|
17
19
|
Attributes:
|
18
|
-
frame_name: Name of the frame to regulate
|
19
|
-
|
20
|
-
|
20
|
+
frame_name: Name of the frame to regulate, typically the name of body, geom
|
21
|
+
or site in the robot model.
|
22
|
+
frame_type: The frame type: `body`, `geom` or `site`.
|
23
|
+
transform_frame_to_world: Target pose of the frame.
|
21
24
|
"""
|
22
25
|
|
23
26
|
k: int = 6
|
@@ -71,21 +74,44 @@ class FrameTask(Task):
|
|
71
74
|
self.cost[3:] = orientation_cost
|
72
75
|
|
73
76
|
def set_target(self, transform_target_to_world: SE3) -> None:
|
74
|
-
"""Set the target pose
|
77
|
+
"""Set the target pose.
|
78
|
+
|
79
|
+
Args:
|
80
|
+
transform_target_to_world: Transform from the task target frame to the
|
81
|
+
world frame.
|
82
|
+
"""
|
75
83
|
self.transform_target_to_world = transform_target_to_world.copy()
|
76
84
|
|
77
85
|
def set_target_from_configuration(self, configuration: Configuration) -> None:
|
78
|
-
"""Set the target pose from a given robot configuration.
|
86
|
+
"""Set the target pose from a given robot configuration.
|
87
|
+
|
88
|
+
Args:
|
89
|
+
configuration: Robot configuration :math:`q`.
|
90
|
+
"""
|
79
91
|
self.set_target(
|
80
92
|
configuration.get_transform_frame_to_world(self.frame_name, self.frame_type)
|
81
93
|
)
|
82
94
|
|
83
95
|
def compute_error(self, configuration: Configuration) -> np.ndarray:
|
84
|
-
"""Compute the frame task error.
|
96
|
+
r"""Compute the frame task error.
|
97
|
+
|
98
|
+
This error is a twist :math:`e(q) \in se(3)` expressed in the local frame,
|
99
|
+
i.e., it is a body twist. It is computed by taking the right-minus difference
|
100
|
+
between the target pose :math:`T_{0t}` and current frame pose :math:`T_{0b}`:
|
85
101
|
|
86
|
-
|
87
|
-
|
88
|
-
|
102
|
+
.. math::
|
103
|
+
|
104
|
+
e(q) := {}_b \xi_{0b} = -(T_{t0} \ominus T_{b0})
|
105
|
+
= -\log(T_{t0} \cdot T_{0b}) = -\log(T_{tb}) = \log(T_{bt})
|
106
|
+
|
107
|
+
where :math:`b` denotes our frame, :math:`t` the target frame and
|
108
|
+
:math:`0` the inertial frame.
|
109
|
+
|
110
|
+
Args:
|
111
|
+
configuration: Robot configuration :math:`q`.
|
112
|
+
|
113
|
+
Returns:
|
114
|
+
Frame task error vector :math:`e(q)`.
|
89
115
|
"""
|
90
116
|
if self.transform_target_to_world is None:
|
91
117
|
raise TargetNotSet(self.__class__.__name__)
|
@@ -96,10 +122,16 @@ class FrameTask(Task):
|
|
96
122
|
return self.transform_target_to_world.minus(transform_frame_to_world)
|
97
123
|
|
98
124
|
def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
|
99
|
-
"""Compute the frame task Jacobian.
|
125
|
+
r"""Compute the frame task Jacobian.
|
126
|
+
|
127
|
+
The derivation of the formula for this Jacobian is detailed in
|
128
|
+
[FrameTaskJacobian]_.
|
129
|
+
|
130
|
+
Args:
|
131
|
+
configuration: Robot configuration :math:`q`.
|
100
132
|
|
101
|
-
|
102
|
-
|
133
|
+
Returns:
|
134
|
+
Frame task jacobian :math:`J(q)`.
|
103
135
|
"""
|
104
136
|
if self.transform_target_to_world is None:
|
105
137
|
raise TargetNotSet(self.__class__.__name__)
|
mink/tasks/posture_task.py
CHANGED
@@ -1,3 +1,7 @@
|
|
1
|
+
"""Posture task implementation."""
|
2
|
+
|
3
|
+
from __future__ import annotations
|
4
|
+
|
1
5
|
from typing import Optional
|
2
6
|
|
3
7
|
import mujoco
|
@@ -11,7 +15,7 @@ from .task import Task
|
|
11
15
|
|
12
16
|
|
13
17
|
class PostureTask(Task):
|
14
|
-
"""Regulate joint angles
|
18
|
+
"""Regulate the joint angles of the robot towards a desired posture.
|
15
19
|
|
16
20
|
A posture is a vector of actuated joint angles. Floating-base coordinates are not
|
17
21
|
affected by this task.
|
@@ -50,7 +54,11 @@ class PostureTask(Task):
|
|
50
54
|
self.nq = model.nq
|
51
55
|
|
52
56
|
def set_target(self, target_q: npt.ArrayLike) -> None:
|
53
|
-
"""Set the target posture.
|
57
|
+
"""Set the target posture.
|
58
|
+
|
59
|
+
Args:
|
60
|
+
target_q: Desired joint configuration.
|
61
|
+
"""
|
54
62
|
target_q = np.atleast_1d(target_q)
|
55
63
|
if target_q.ndim != 1 or target_q.shape[0] != (self.nq):
|
56
64
|
raise InvalidTarget(
|
@@ -60,14 +68,27 @@ class PostureTask(Task):
|
|
60
68
|
self.target_q = target_q.copy()
|
61
69
|
|
62
70
|
def set_target_from_configuration(self, configuration: Configuration) -> None:
|
63
|
-
"""Set the target posture from the current configuration.
|
71
|
+
"""Set the target posture from the current configuration.
|
72
|
+
|
73
|
+
Args:
|
74
|
+
configuration: Robot configuration :math:`q`.
|
75
|
+
"""
|
64
76
|
self.set_target(configuration.q)
|
65
77
|
|
66
78
|
def compute_error(self, configuration: Configuration) -> np.ndarray:
|
67
|
-
"""Compute the posture task error.
|
79
|
+
r"""Compute the posture task error.
|
80
|
+
|
81
|
+
The error is defined as:
|
68
82
|
|
69
|
-
|
70
|
-
|
83
|
+
.. math::
|
84
|
+
|
85
|
+
e(q) = q^* \ominus q
|
86
|
+
|
87
|
+
Args:
|
88
|
+
configuration: Robot configuration :math:`q`.
|
89
|
+
|
90
|
+
Returns:
|
91
|
+
Posture task error vector :math:`e(q)`.
|
71
92
|
"""
|
72
93
|
if self.target_q is None:
|
73
94
|
raise TargetNotSet(self.__class__.__name__)
|
@@ -88,10 +109,19 @@ class PostureTask(Task):
|
|
88
109
|
return qvel
|
89
110
|
|
90
111
|
def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
|
91
|
-
"""Compute the posture task Jacobian.
|
112
|
+
r"""Compute the posture task Jacobian.
|
113
|
+
|
114
|
+
The task Jacobian is defined as:
|
115
|
+
|
116
|
+
.. math::
|
117
|
+
|
118
|
+
J(q) = I_{n_v}
|
119
|
+
|
120
|
+
Args:
|
121
|
+
configuration: Robot configuration :math:`q`.
|
92
122
|
|
93
|
-
|
94
|
-
|
123
|
+
Returns:
|
124
|
+
Posture task jacobian :math:`J(q)`.
|
95
125
|
"""
|
96
126
|
if self.target_q is None:
|
97
127
|
raise TargetNotSet(self.__class__.__name__)
|
mink/tasks/task.py
CHANGED
@@ -1,3 +1,5 @@
|
|
1
|
+
"""All kinematic tasks derive from the :class:`Task` base class."""
|
2
|
+
|
1
3
|
import abc
|
2
4
|
from typing import NamedTuple
|
3
5
|
|
@@ -8,10 +10,12 @@ from .exceptions import InvalidDamping, InvalidGain
|
|
8
10
|
|
9
11
|
|
10
12
|
class Objective(NamedTuple):
|
11
|
-
"""Quadratic objective
|
13
|
+
r"""Quadratic objective of the form :math:`\frac{1}{2} x^T H x + c^T x`."""
|
12
14
|
|
13
|
-
H: np.ndarray
|
14
|
-
|
15
|
+
H: np.ndarray
|
16
|
+
"""Hessian matrix, of shape (n_v, n_v)"""
|
17
|
+
c: np.ndarray
|
18
|
+
"""Linear vector, of shape (n_v,)."""
|
15
19
|
|
16
20
|
def value(self, x: np.ndarray) -> float:
|
17
21
|
"""Returns the value of the objective at the input vector."""
|
@@ -19,7 +23,33 @@ class Objective(NamedTuple):
|
|
19
23
|
|
20
24
|
|
21
25
|
class Task(abc.ABC):
|
22
|
-
"""Abstract base class for kinematic tasks.
|
26
|
+
r"""Abstract base class for kinematic tasks.
|
27
|
+
|
28
|
+
Subclasses must implement the configuration-dependent task error
|
29
|
+
:py:meth:`~Task.compute_error` and Jacobian :py:meth:`~Task.compute_jacobian`
|
30
|
+
functions.
|
31
|
+
|
32
|
+
The error function :math:`e(q) \in \mathbb{R}^{k}` is the quantity that
|
33
|
+
the task aims to drive to zero (:math:`k` is the dimension of the
|
34
|
+
task). It appears in the first-order task dynamics:
|
35
|
+
|
36
|
+
.. math::
|
37
|
+
|
38
|
+
J(q) \Delta q = -\alpha e(q)
|
39
|
+
|
40
|
+
The Jacobian matrix :math:`J(q) \in \mathbb{R}^{k \times n_v}`, with
|
41
|
+
:math:`n_v` the dimension of the robot's tangent space, is the
|
42
|
+
derivative of the task error :math:`e(q)` with respect to the
|
43
|
+
configuration :math:`q \in \mathbb{R}^{n_q}`. The configuration displacement
|
44
|
+
:math:`\Delta q` is the output of inverse kinematics; we divide it by dt to get a
|
45
|
+
commanded velocity.
|
46
|
+
|
47
|
+
In the first-order task dynamics, the error :math:`e(q)` is multiplied
|
48
|
+
by the task gain :math:`\alpha \in [0, 1]`. This gain can be 1.0 for
|
49
|
+
dead-beat control (*i.e.* converge as fast as possible), but might be
|
50
|
+
unstable as it neglects our first-order approximation. Lower values
|
51
|
+
cause slow down the task, similar to low-pass filtering.
|
52
|
+
"""
|
23
53
|
|
24
54
|
def __init__(
|
25
55
|
self,
|
@@ -41,7 +71,7 @@ class Task(abc.ABC):
|
|
41
71
|
if not 0.0 <= gain <= 1.0:
|
42
72
|
raise InvalidGain("`gain` must be in the range [0, 1]")
|
43
73
|
|
44
|
-
if
|
74
|
+
if lm_damping < 0.0:
|
45
75
|
raise InvalidDamping("`lm_damping` must be >= 0")
|
46
76
|
|
47
77
|
self.cost = cost
|
@@ -50,23 +80,13 @@ class Task(abc.ABC):
|
|
50
80
|
|
51
81
|
@abc.abstractmethod
|
52
82
|
def compute_error(self, configuration: Configuration) -> np.ndarray:
|
53
|
-
"""Compute the task error
|
54
|
-
|
55
|
-
The error function e(q), of shape (k,), is the quantity that the task aims to
|
56
|
-
drive to zero. It appears in the first-order task dynamics:
|
57
|
-
|
58
|
-
J(q) Δq = -alpha e(q)
|
83
|
+
"""Compute the task error at the current configuration.
|
59
84
|
|
60
|
-
|
61
|
-
|
62
|
-
`compute_jacobian`. Finally, the configuration displacement Δq is the output
|
63
|
-
of inverse kinematics.
|
85
|
+
Args:
|
86
|
+
configuration: Robot configuration :math:`q`.
|
64
87
|
|
65
|
-
|
66
|
-
|
67
|
-
(i.e., converge as fast as possible), but might be unstable as it neglects the
|
68
|
-
first-order approximation. Lower values make the task slower and have a similar
|
69
|
-
effect to low-pass filtering.
|
88
|
+
Returns:
|
89
|
+
Task error vector :math:`e(q)`.
|
70
90
|
"""
|
71
91
|
raise NotImplementedError
|
72
92
|
|
@@ -74,20 +94,33 @@ class Task(abc.ABC):
|
|
74
94
|
def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
|
75
95
|
"""Compute the task Jacobian at the current configuration.
|
76
96
|
|
77
|
-
|
78
|
-
|
79
|
-
|
97
|
+
Args:
|
98
|
+
configuration: Robot configuration :math:`q`.
|
99
|
+
|
100
|
+
Returns:
|
101
|
+
Task jacobian :math:`J(q)`.
|
80
102
|
"""
|
81
103
|
raise NotImplementedError
|
82
104
|
|
83
105
|
def compute_qp_objective(self, configuration: Configuration) -> Objective:
|
84
|
-
"""Compute the matrix-vector pair :math:`(H, c)` of the QP objective.
|
106
|
+
r"""Compute the matrix-vector pair :math:`(H, c)` of the QP objective.
|
107
|
+
|
108
|
+
This pair is such that the contribution of the task to the QP objective is:
|
85
109
|
|
86
|
-
|
87
|
-
|
110
|
+
.. math::
|
111
|
+
|
112
|
+
\| J \Delta q + \alpha e \|_{W}^2 = \frac{1}{2} \Delta q^T H
|
113
|
+
\Delta q + c^T q
|
114
|
+
|
115
|
+
The weight matrix :math:`W \in \mathbb{R}^{k \times k}` weights and
|
116
|
+
normalizes task coordinates to the same unit. The unit of the overall
|
117
|
+
contribution is [cost]^2.
|
118
|
+
|
119
|
+
Args:
|
120
|
+
configuration: Robot configuration :math:`q`.
|
88
121
|
|
89
|
-
|
90
|
-
|
122
|
+
Returns:
|
123
|
+
Pair :math:`(H(q), c(q))`.
|
91
124
|
"""
|
92
125
|
jacobian = self.compute_jacobian(configuration) # (k, nv)
|
93
126
|
minus_gain_error = -self.gain * self.compute_error(configuration) # (k,)
|
mink/utils.py
CHANGED
@@ -17,8 +17,8 @@ def move_mocap_to_frame(
|
|
17
17
|
"""Initialize mocap body pose at a desired frame.
|
18
18
|
|
19
19
|
Args:
|
20
|
-
model:
|
21
|
-
data:
|
20
|
+
model: Mujoco model.
|
21
|
+
data: Mujoco data.
|
22
22
|
mocap_name: The name of the mocap body.
|
23
23
|
frame_name: The desired frame name.
|
24
24
|
frame_type: The desired frame type. Can be "body", "geom" or "site".
|
@@ -36,7 +36,15 @@ def move_mocap_to_frame(
|
|
36
36
|
|
37
37
|
|
38
38
|
def get_freejoint_dims(model: mujoco.MjModel) -> tuple[list[int], list[int]]:
|
39
|
-
"""Get all floating joint configuration and tangent indices.
|
39
|
+
"""Get all floating joint configuration and tangent indices.
|
40
|
+
|
41
|
+
Args:
|
42
|
+
model: Mujoco model.
|
43
|
+
|
44
|
+
Returns:
|
45
|
+
A (q_ids, v_ids) pair containing all floating joint indices in the
|
46
|
+
configuration and tangent spaces respectively.
|
47
|
+
"""
|
40
48
|
q_ids: list[int] = []
|
41
49
|
v_ids: list[int] = []
|
42
50
|
for j in range(model.njnt):
|
@@ -56,9 +64,9 @@ def custom_configuration_vector(
|
|
56
64
|
"""Generate a configuration vector where named joints have specific values.
|
57
65
|
|
58
66
|
Args:
|
59
|
-
model:
|
67
|
+
model: Mujoco model.
|
60
68
|
key_name: Optional keyframe name to initialize the configuration vector from.
|
61
|
-
Otherwise, the default pose qpos0 is used.
|
69
|
+
Otherwise, the default pose `qpos0` is used.
|
62
70
|
kwargs: Custom values for joint coordinates.
|
63
71
|
|
64
72
|
Returns:
|
@@ -93,7 +101,7 @@ def get_subtree_geom_ids(model: mujoco.MjModel, body_id: int) -> list[int]:
|
|
93
101
|
"""Get all geoms belonging to subtree starting at a given body.
|
94
102
|
|
95
103
|
Args:
|
96
|
-
model:
|
104
|
+
model: Mujoco model.
|
97
105
|
body_id: ID of body where subtree starts.
|
98
106
|
|
99
107
|
Returns:
|
@@ -117,7 +125,7 @@ def get_body_geom_ids(model: mujoco.MjModel, body_id: int) -> list[int]:
|
|
117
125
|
"""Get all geoms belonging to a given body.
|
118
126
|
|
119
127
|
Args:
|
120
|
-
model:
|
128
|
+
model: Mujoco model.
|
121
129
|
body_id: ID of body.
|
122
130
|
|
123
131
|
Returns:
|
@@ -1,6 +1,6 @@
|
|
1
1
|
Metadata-Version: 2.1
|
2
2
|
Name: mink
|
3
|
-
Version: 0.0.
|
3
|
+
Version: 0.0.2
|
4
4
|
Summary: mink: MuJoCo inverse kinematics.
|
5
5
|
Keywords: inverse,kinematics,mujoco
|
6
6
|
Author-email: Kevin Zakka <zakka@berkeley.edu>
|
@@ -11,26 +11,31 @@ Classifier: Framework :: Robot Framework :: Library
|
|
11
11
|
Classifier: Intended Audience :: Developers
|
12
12
|
Classifier: Intended Audience :: Science/Research
|
13
13
|
Classifier: License :: OSI Approved :: Apache Software License
|
14
|
+
Classifier: Programming Language :: Python :: 3.9
|
14
15
|
Classifier: Programming Language :: Python :: 3.10
|
15
16
|
Classifier: Programming Language :: Python :: 3.11
|
16
17
|
Classifier: Programming Language :: Python :: 3.12
|
17
18
|
Classifier: Topic :: Scientific/Engineering
|
18
19
|
Requires-Dist: mujoco >= 3.1.6
|
19
|
-
Requires-Dist: qpsolvers >= 4.3.1
|
20
|
-
Requires-Dist: quadprog >= 0.1.11
|
20
|
+
Requires-Dist: qpsolvers[quadprog] >= 4.3.1
|
21
21
|
Requires-Dist: typing_extensions
|
22
|
-
Requires-Dist:
|
23
|
-
Requires-Dist:
|
22
|
+
Requires-Dist: numpy < 2.0.0
|
23
|
+
Requires-Dist: mink[examples,dev] ; extra == "all"
|
24
24
|
Requires-Dist: black ; extra == "dev"
|
25
25
|
Requires-Dist: mink[test] ; extra == "dev"
|
26
|
-
Requires-Dist:
|
26
|
+
Requires-Dist: mypy ; extra == "dev"
|
27
|
+
Requires-Dist: ruff ; extra == "dev"
|
27
28
|
Requires-Dist: dm_control >= 1.0.20 ; extra == "examples"
|
29
|
+
Requires-Dist: loop-rate-limiters >= 0.1.0 ; extra == "examples"
|
30
|
+
Requires-Dist: qpsolvers[quadprog,osqp] >= 4.3.1 ; extra == "examples"
|
28
31
|
Requires-Dist: absl-py ; extra == "test"
|
32
|
+
Requires-Dist: coveralls ; extra == "test"
|
29
33
|
Requires-Dist: pytest ; extra == "test"
|
30
34
|
Requires-Dist: robot_descriptions >= 1.9.0 ; extra == "test"
|
31
35
|
Project-URL: Changelog, https://github.com/kevinzakka/mink/blob/main/CHANGELOG.md
|
32
36
|
Project-URL: Source, https://github.com/kevinzakka/mink
|
33
37
|
Project-URL: Tracker, https://github.com/kevinzakka/mink/issues
|
38
|
+
Provides-Extra: all
|
34
39
|
Provides-Extra: dev
|
35
40
|
Provides-Extra: examples
|
36
41
|
Provides-Extra: test
|
@@ -38,21 +43,43 @@ Provides-Extra: test
|
|
38
43
|
# mink
|
39
44
|
|
40
45
|
[](https://github.com/kevinzakka/mink/actions)
|
46
|
+
[](https://coveralls.io/github/kevinzakka/mink)
|
47
|
+
[](https://pypi.org/project/mink/)
|
48
|
+

|
41
49
|
|
42
50
|
mink is a library for differential inverse kinematics in Python, based on the [MuJoCo](https://github.com/google-deepmind/mujoco) physics engine.
|
43
51
|
|
52
|
+
Features include:
|
53
|
+
|
54
|
+
* Task specification in configuration or operational space;
|
55
|
+
* Limits on joint positions and velocities;
|
56
|
+
* Collision avoidance between any geom pair;
|
57
|
+
* Lie group interface for rigid body transformations.
|
58
|
+
|
59
|
+
For usage and API reference, see the [documentation](https://kevinzakka.github.io/mink/).
|
60
|
+
|
44
61
|
## Installation
|
45
62
|
|
63
|
+
You can install `mink` using `pip`:
|
64
|
+
|
46
65
|
```bash
|
47
|
-
pip install
|
66
|
+
pip install mink
|
67
|
+
```
|
68
|
+
|
69
|
+
To include the example dependencies:
|
70
|
+
|
71
|
+
```bash
|
72
|
+
pip install "mink[examples]"
|
48
73
|
```
|
49
74
|
|
50
75
|
## Examples
|
51
76
|
|
77
|
+
mink works with a variety of robots, including:
|
78
|
+
|
52
79
|
* Arms: [UR5e](https://github.com/kevinzakka/mink/blob/main/examples/arm_ur5e_actuators.py), [iiwa14](https://github.com/kevinzakka/mink/blob/main/examples/arm_iiwa.py), [bimanual iiwa14](https://github.com/kevinzakka/mink/blob/main/examples/dual_iiwa.py)
|
53
80
|
* Humanoids: [Unitree G1](https://github.com/kevinzakka/mink/blob/main/examples/humanoid_g1.py)
|
54
81
|
* Quadrupeds: [Unitree Go1](https://github.com/kevinzakka/mink/blob/main/examples/quadruped_go1.py), [Boston Dynamics Spot](https://github.com/kevinzakka/mink/blob/main/examples/quadruped_spot.py)
|
55
|
-
* Hands: [Shadow Hand]()
|
82
|
+
* Hands: [Shadow Hand](https://github.com/kevinzakka/mink/blob/main/examples/hand_shadow.py)
|
56
83
|
|
57
84
|
Check out the [examples](https://github.com/kevinzakka/mink/blob/main/examples/) directory for more code.
|
58
85
|
|
@@ -62,7 +89,7 @@ Install the library, use it and report any bugs in the [issue tracker](https://g
|
|
62
89
|
|
63
90
|
## Acknowledgements
|
64
91
|
|
65
|
-
mink is a direct port of [
|
92
|
+
mink is a direct port of [Pink](https://github.com/stephane-caron/pink) which uses [Pinocchio](https://github.com/stack-of-tasks/pinocchio) under the hood. Stéphane Caron, the author of Pink, is a role model for open-source software in robotics. This library would not have been possible without his work and assistance throughout this project.
|
66
93
|
|
67
94
|
mink also heavily adapts code from the following libraries:
|
68
95
|
|
@@ -1,10 +1,10 @@
|
|
1
|
-
mink/__init__.py,sha256=
|
2
|
-
mink/configuration.py,sha256=
|
1
|
+
mink/__init__.py,sha256=8EUZVesmJRHjCqwB-h-NA8l7pb1o0ou0Efeg3rVIvlU,1692
|
2
|
+
mink/configuration.py,sha256=V1Kpc5oW7Ts-db1mjAU8O-OgkjFVjKRIJ1IX-56vZ2s,8555
|
3
3
|
mink/constants.py,sha256=hcWy4zM4hGI7vvJrTmCjJgs2WtOxKJ1IVtYHucWbOAI,813
|
4
4
|
mink/exceptions.py,sha256=tmExo-ydn_mu9Mny5sP0Ehr7hfo3d79fM8HvvxZBKYM,2931
|
5
5
|
mink/py.typed,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
6
6
|
mink/solve_ik.py,sha256=0ljN-CXDZFX8dPDvtyQxlI4vGoRpmeWmBkLT1-999Rw,3542
|
7
|
-
mink/utils.py,sha256
|
7
|
+
mink/utils.py,sha256=-00C_AmtfvkmmWiHpds5Fu83gLcnt9OJ2KmyB01aTa8,4387
|
8
8
|
mink/.mypy_cache/.gitignore,sha256=amnaZw0RUw038PDP3HvtMLeOpkNOJPenMgi5guKdMiw,34
|
9
9
|
mink/.mypy_cache/CACHEDIR.TAG,sha256=8cE6_FVTWMkDOw8fMKqhd_6IvaQPS4okWYQA1UeHatw,190
|
10
10
|
mink/.mypy_cache/3.12/@plugins_snapshot.json,sha256=RBNvo1WzZ4oRRq0W9-hknpT7T8If536DEMBg9hyq_4o,2
|
@@ -327,26 +327,27 @@ mink/.mypy_cache/3.12/unittest/signals.meta.json,sha256=t4kGxxcZb9aDLojvZ5naLIcc
|
|
327
327
|
mink/.mypy_cache/3.12/unittest/suite.data.json,sha256=3XPU9U8rt00fnG-hgEIbpIqS14lI9xQMDiBsl2_mlXQ,12050
|
328
328
|
mink/.mypy_cache/3.12/unittest/suite.meta.json,sha256=5ADDHudtGO-JtLaD0-SB-7_Ekm34XeG_QCXLEDIZeIM,1699
|
329
329
|
mink/lie/__init__.py,sha256=tI-lfqTHT8iABKemXZCZUEtiDtj4LC9o7DEDXpkn6tI,228
|
330
|
-
mink/lie/base.py,sha256=
|
330
|
+
mink/lie/base.py,sha256=ummp2-yROMrcYCtsNCoKEiKoJ7wLF0nSjnOOD95aXaY,4220
|
331
331
|
mink/lie/se3.py,sha256=R4hH4i0-do5I8bWV0le-huSuAfQ_bwolkAjJNVlDNXs,7914
|
332
|
-
mink/lie/so3.py,sha256=
|
332
|
+
mink/lie/so3.py,sha256=LkjciJHcVTWYzpDzAo1KuUI7xy0HPnSGAYxQS_q-Kx4,7302
|
333
333
|
mink/lie/utils.py,sha256=Drx_l0adSLlZl89kVLRZxMW4ctafG1pk0iucS7gShOI,608
|
334
334
|
mink/lie/tests/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
335
335
|
mink/lie/tests/test_axioms.py,sha256=Vgkwoa-GtqUgiI8A5F041NfdOkP4Gxj6wUdNzHzuuFE,1756
|
336
336
|
mink/lie/tests/test_operations.py,sha256=UTUyYA-jDm73LCe8KWOcQo0PFx45UDDm25Y1J03FVhk,2664
|
337
337
|
mink/lie/tests/utils.py,sha256=N6B86t7BRH_Ls5yykCD4llVM2ElyXp9ze2RyLPMBUY4,562
|
338
338
|
mink/limits/__init__.py,sha256=hX5Dgpri9AE6YtUCkW79AXMBuNAuBhniR9kQ6Rxwv3Y,416
|
339
|
-
mink/limits/collision_avoidance_limit.py,sha256=
|
340
|
-
mink/limits/configuration_limit.py,sha256=
|
339
|
+
mink/limits/collision_avoidance_limit.py,sha256=k7_FfoYlPRxhYfxmGt03JiCU0J4kIMgfnqQYT3qZpn8,11765
|
340
|
+
mink/limits/configuration_limit.py,sha256=8GlOIRWHJUX9QCGrmR6YEYkpXntnQxWjd5oz2r8qUb4,4037
|
341
341
|
mink/limits/exceptions.py,sha256=EnVKgFhUJFM6rNVCtdngb-XMu66PWKMFSDy-XkdKzr8,178
|
342
|
-
mink/limits/limit.py,sha256=
|
343
|
-
mink/limits/velocity_limit.py,sha256=
|
344
|
-
mink/tasks/__init__.py,sha256=
|
345
|
-
mink/tasks/com_task.py,sha256=
|
342
|
+
mink/limits/limit.py,sha256=feF9yjgLHWShnuSrJNOH-PfMWtEMRGQi2mDiRia9tFg,1578
|
343
|
+
mink/limits/velocity_limit.py,sha256=2zPOAb69V9RusKk-U4cJ0lK3-Ek9g4nvr3c8QHbixzw,3506
|
344
|
+
mink/tasks/__init__.py,sha256=mD7kGU22-mIqkK2ZGUsglqxiLC-KePV7SYfh8Xh63TM,548
|
345
|
+
mink/tasks/com_task.py,sha256=ch6w7pwa5E0GVo2DCAuEF-c8TCN1iMk-ySY9A4B6yoQ,3121
|
346
|
+
mink/tasks/damping_task.py,sha256=_o8Uxt9a_MHcW7e_j5wuSPPeZhRZyN88YjwGtZbH3hM,521
|
346
347
|
mink/tasks/exceptions.py,sha256=1BxQS_t8vuKoTd8RY0MdZOJ_-2RX--iUtVs-Bu3IVgs,708
|
347
|
-
mink/tasks/frame_task.py,sha256=
|
348
|
-
mink/tasks/posture_task.py,sha256=
|
349
|
-
mink/tasks/task.py,sha256=
|
350
|
-
mink-0.0.
|
351
|
-
mink-0.0.
|
352
|
-
mink-0.0.
|
348
|
+
mink/tasks/frame_task.py,sha256=DeNg-bEJxqnrRI0FaJK4UHyb8CyF4A7uPF9G-CdN0sg,5307
|
349
|
+
mink/tasks/posture_task.py,sha256=KuDXctRlRXaxqtL2fHNijk5acKLWL27X7Dtyb6VZeuk,3548
|
350
|
+
mink/tasks/task.py,sha256=F16YZT1L9ueNOcKoOhbCyEnZw0DOgrmjqADl0jahVQI,4838
|
351
|
+
mink-0.0.2.dist-info/WHEEL,sha256=EZbGkh7Ie4PoZfRQ8I0ZuP9VklN_TvcZ6DSE5Uar4z4,81
|
352
|
+
mink-0.0.2.dist-info/METADATA,sha256=FYHlYmtZa5zrnHl0tomLx0RJi0Vt0bAvl8P6-74dsP8,4642
|
353
|
+
mink-0.0.2.dist-info/RECORD,,
|
File without changes
|