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 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 ComTask, FrameTask, Objective, PostureTask, TargetNotSet, Task
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.1"
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
- """A struct that provides convenient access to kinematic quantities such as frame
13
- transforms and frame jacobians. Frames can be defined at bodies, geoms or sites.
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
- The `update` function ensures the proper forward kinematics functions have been
16
- called, namely:
34
+ Key functionalities include:
17
35
 
18
- - mujoco.mj_kinematics(model, data)
19
- - mujoco.mj_comPos(model, data)
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: An instance of MjModel.
31
- q: Optional configuration to initialize from. If None, the configuration
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
- The minimal function call required to get updated frame transforms (aka forward
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 out of limits: "
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, the Jacobian matrix B_J_WB
106
- is related to the body velocity B_v_WB by:
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
- B_v_WB = B_J_WB q_dot
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 B_J_WB of the frame.
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 in the current configuration.
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:: Dimension of underlying parameters.
14
- tangent_dim:: Dimension of tangent space.
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
@@ -14,6 +14,8 @@ _INVERT_QUAT_SIGN = np.array([1.0, -1.0, -1.0, -1.0], dtype=np.float64)
14
14
 
15
15
 
16
16
  class RollPitchYaw(NamedTuple):
17
+ """Struct containing roll, pitch, and yaw Euler angles."""
18
+
17
19
  roll: float
18
20
  pitch: float
19
21
  yaw: float
@@ -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 Contact:
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 to tighten or relax
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
- ) -> Contact:
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 Contact(
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: 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
- """Limit on joint positions.
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
- """Limit for joint positions in a model.
34
-
35
- Floating base joints (joint type="free") are ignored.
13
+ """Inequality constraint on joint positions in a robot model.
36
14
 
37
- Attributes:
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 inequalities in the form G(q) dq <= h(q).
12
+ r"""Linear inequality constraint of the form :math:`G(q) \Delta q \leq h(q)`.
11
13
 
12
- The limit is considered inactive when G or h are None.
14
+ Inactive if G and h are None.
13
15
  """
14
16
 
15
- G: Optional[np.ndarray] = None # (nv, nv)
16
- h: Optional[np.ndarray] = None # (nv,)
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
- return self.G is None or self.h is None
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: Current configuration.
53
+ configuration: Robot configuration :math:`q`.
36
54
  dt: Integration time step in [s].
37
55
 
38
56
  Returns:
39
- Pair (G, h) representing the inequality constraint.
57
+ Pair :math:`(G, h)`.
40
58
  """
41
59
  raise NotImplementedError
@@ -1,24 +1,4 @@
1
- """Limit on joint velocities.
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
- """Limit for joint velocities in a model.
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 of mass task implementation."""
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 of mass (CoM) of a robot.
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
- The error is the difference between the target CoM and current CoM positions,
66
- expressed in the world frame.
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
- The task Jacobian is the derivative of the task error with respect to the
76
- current configuration. It has dimension (3, nv).
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 robot frame in the world frame.
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
- frame_type: The frame type: body, geom or site.
20
- transform_frame_to_world: Target pose of the frame in the world frame.
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 in the world frame."""
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
- This error is a twist expressed in the local frame, i.e., it is a body twist.
87
- It is computed by taking the right minus difference between the target pose
88
- and the current frame pose: `e = T_wt T_wf`.
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
- The task Jacobian is the derivative of the task error with respect to the
102
- current configuration. It has dimension (6, nv).
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__)
@@ -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 to a desired posture.
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
- The error is defined as the right minus error between the target posture and
70
- the current posture: `q^* ⊖ q`.
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
- The task Jacobian is the derivative of the task error with respect to the
94
- current configuration. It is equal to -I and has dimension (nv, nv).
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 function in the form 0.5 x^T H x + c^T x."""
13
+ r"""Quadratic objective of the form :math:`\frac{1}{2} x^T H x + c^T x`."""
12
14
 
13
- H: np.ndarray # (nv, nv)
14
- c: np.ndarray # (nv,)
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 not lm_damping >= 0.0:
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 function at the current configuration.
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
- The Jacobian matrix J(q), of shape (k, nv), is the derivative of the task error
61
- with respect to the configuration. This Jacobian is implemented in
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
- In the first-order task dynamics, the error e(q) is multiplied by the task gain
66
- alpha which is between [0, 1]. This gain can be 1.0 for dead-beat control
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
- The task Jacobian J(q), of shape (k, nv) is the first-order derivative of the
78
- error e(q) that defines the task, with k the dimension of the task and
79
- nv the dimension of the robot's tangent space.
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
- This pair is such that the contribution of the task to the QP objective
87
- of the IK is:
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
- The weight matrix W, of shape (k, k), weighs and normalizes task coordinates
90
- to the same unit. The unit of the overall contribution is [cost]^2.
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: An instance of mujoco.MjModel.
21
- data: An instance of mujoco.MjData.
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: An MjModel instance.
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: An MjModel instance.
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: An MjModel instance.
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.1
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: mypy ; extra == "dev"
23
- Requires-Dist: ruff ; extra == "dev"
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: loop-rate-limiters >= 0.1.0 ; extra == "examples"
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
  [![Build](https://img.shields.io/github/actions/workflow/status/kevinzakka/mink/ci.yml?branch=main)](https://github.com/kevinzakka/mink/actions)
46
+ [![Coverage Status](https://coveralls.io/repos/github/kevinzakka/mink/badge.svg)](https://coveralls.io/github/kevinzakka/mink)
47
+ [![PyPI version](https://img.shields.io/pypi/v/mink)](https://pypi.org/project/mink/)
48
+ ![Banner for mink](https://github.com/kevinzakka/mink/blob/assets/banner.png?raw=true)
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 -e ".[examples]"
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 [pink](https://github.com/stephane-caron/pink) which uses [Pinocchio](https://github.com/stack-of-tasks/pinocchio) under the hood. Stéphane Caron, the original 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.
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=qLYRMUEsu-XusX2bvM_S3MJhYwYM-u3fvizqYur69XI,1627
2
- mink/configuration.py,sha256=6ScxC-mQwaSJ7Vp5Ofcms12qxgPUIMQ_0DacW6w_XwU,7490
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=vU3huZtKY_RWYjZRez7iZbXaagXIfGa_E6KZGbJ-oo0,4251
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=p9PoZBZ-6vCGUN0kiguz2ActGtpXRInltGU2Vk9bu54,4222
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=ii3jYqy3KrOrNR0GLA3w5OBvujUpgRg0BXa7cPXXwFE,7238
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=vwi3CBdwkc04_z97xanol5Bi2khMbI94lP63ZzC0uXo,11177
340
- mink/limits/configuration_limit.py,sha256=I-4U2oXupdfgdQUWhDrrNmyvK58ZdrIxiOCSJjbZ-HY,3995
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=1uNmQGcYuiLQ4q9AVEZzzyekc-kQtx9d0HKKSH8_7ks,978
343
- mink/limits/velocity_limit.py,sha256=8q7QuBdcN5msspsz24MKGZSinrFlZEIHsby62PMtF-k,2935
344
- mink/tasks/__init__.py,sha256=e_6bfrOKuYdInaqd-7uPEXVWhWJH71R5UF73DaF59Pc,491
345
- mink/tasks/com_task.py,sha256=GvHvj1nuz_tTxglvqHL9N4VlHuNDCAHh7rgXAgrbrVY,2926
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=QpavqXsShhv89EeomXu9HhSqkX_MWqEaPO2Q8Y-Ik68,4472
348
- mink/tasks/posture_task.py,sha256=jMBmDiavG05RutJzJ4QO6xgSuSqCFbAx9bzRT8JqcjU,3125
349
- mink/tasks/task.py,sha256=bkhqO76lS1cgx4gJbXnJvD4WPLovTSQ3gDH8D0QAMd0,4074
350
- mink-0.0.1.dist-info/WHEEL,sha256=EZbGkh7Ie4PoZfRQ8I0ZuP9VklN_TvcZ6DSE5Uar4z4,81
351
- mink-0.0.1.dist-info/METADATA,sha256=mAfZ_hgRpS8vVrvz6Z_t5uoEet0jki28ILFjFuLpvHY,3575
352
- mink-0.0.1.dist-info/RECORD,,
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