Skip to content

Commit 0709d20

Browse files
committed
Docstring improvements.
1 parent 63e099f commit 0709d20

File tree

6 files changed

+38
-29
lines changed

6 files changed

+38
-29
lines changed

mink/solve_ik.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ def build_ik(
4848
damping: float = 1e-12,
4949
limits: Optional[Sequence[Limit]] = None,
5050
) -> qpsolvers.Problem:
51-
r"""Build quadratic program from current configuration and tasks.
51+
r"""Build the quadratic program given the current configuration and tasks.
5252
5353
The quadratic program is defined as:
5454

mink/tasks/com_task.py

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,12 @@ def __init__(
4949
self.set_cost(cost)
5050

5151
def set_cost(self, cost: npt.ArrayLike) -> None:
52+
"""Set the cost of the CoM task.
53+
54+
Args:
55+
cost: A vector of shape (1,) (aka identical cost for all coordinates),
56+
or (3,) (aka different costs for each coordinate).
57+
"""
5258
cost = np.atleast_1d(cost)
5359
if cost.ndim != 1 or cost.shape[0] not in (1, self.k):
5460
raise TaskDefinitionError(
@@ -64,7 +70,8 @@ def set_target(self, target_com: npt.ArrayLike) -> None:
6470
"""Set the target CoM position in the world frame.
6571
6672
Args:
67-
target_com: Desired center-of-mass position in the world frame.
73+
target_com: A vector of shape (3,) representing the desired
74+
center-of-mass position in the world frame.
6875
"""
6976
target_com = np.atleast_1d(target_com)
7077
if target_com.ndim != 1 or target_com.shape[0] != (self.k):

mink/tasks/damping_task.py

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -11,26 +11,26 @@
1111
class DampingTask(PostureTask):
1212
r"""L2-regularization on joint velocities (a.k.a. *velocity damping*).
1313
14-
This low-priority task adds a Tikhonov/Levenberg-Marquardt term to the
15-
quadratic program, making the Hessian strictly positive-definite and
16-
selecting the **minimum-norm joint velocity** in any redundant or
17-
near-singular situation. Formally it contributes:
14+
This task, typically used with a low priority in the task stack, adds a
15+
Levenberg-Marquardt term to the quadratic program, favoring **minimum-norm
16+
joint velocities** in redundant or near-singular situations. Formally, it
17+
contributes the following term to the quadratic program:
1818
1919
.. math::
2020
\frac{1}{2}\,\Delta \mathbf{q}^\top \Lambda\,\Delta \mathbf{q},
2121
2222
where :math:`\Delta \mathbf{q}\in\mathbb{R}^{n_v}` is the vector of joint
2323
displacements and :math:`\Lambda = \mathrm{diag}(\lambda_1, \ldots, \lambda_{n_v})`
24-
is a diagonal matrix of per-DoF weights provided by ``cost``. A larger
25-
:math:`\lambda_i` reduces motion in DoF :math:`i`; with no other active
26-
tasks the robot remains at rest.
27-
28-
This task does not favor a particular posture—only small instantaneous
29-
motion. If you need a posture bias, use an explicit :class:`~.PostureTask`.
24+
is a diagonal matrix of per-DoF damping weights specified via ``cost``. A larger
25+
:math:`\lambda_i` reduces motion in DoF :math:`i`. With no other active
26+
tasks, the robot remains at rest. Unlike the `damping` parameter in
27+
:func:`~.solve_ik`, which is uniformly applied to all DoFs, this task does
28+
not affect the floating-base coordinates.
3029
3130
.. note::
3231
33-
Floating-base coordinates are not affected by this task.
32+
This task does not favor a particular posture, only small instantaneous motion.
33+
If you need a posture bias, use :class:`~.PostureTask` instead.
3434
3535
Example:
3636
@@ -53,7 +53,7 @@ def compute_error(self, configuration: Configuration) -> np.ndarray:
5353
r"""Compute the damping task error.
5454
5555
The damping task does not chase a reference; its desired joint velocity
56-
is identically **zero**, so the error vector is always
56+
is identically **zero**, so the task error is always:
5757
5858
.. math:: e = \mathbf 0 \in \mathbb R^{n_v}.
5959

mink/tasks/frame_task.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,13 +14,13 @@
1414

1515

1616
class FrameTask(Task):
17-
"""Regulate the position and orientation of a frame expressed in the world frame.
17+
"""Regulate the position and orientation of a frame of interest on the robot.
1818
1919
Attributes:
2020
frame_name: Name of the frame to regulate, typically the name of body, geom
2121
or site in the robot model.
2222
frame_type: The frame type: `body`, `geom` or `site`.
23-
transform_frame_to_world: Target pose of the frame.
23+
transform_frame_to_world: Target pose of the frame in the world frame.
2424
2525
Example:
2626

mink/tasks/kinetic_energy_regularization_task.py

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -12,23 +12,24 @@
1212

1313

1414
class KineticEnergyRegularizationTask(BaseTask):
15-
r"""Kinetic-energy regularization task.
15+
r"""Kinetic-energy regularization.
1616
17-
This low-priority task adds a configuration-dependent quadratic term to the
18-
QP objective that penalizes the system's kinetic energy. Formally, it contributes:
17+
This task, often used with a low priority in the task stack, penalizes the system's
18+
kinetic energy. Formally, it contributes the following term to the quadratic
19+
program:
1920
2021
.. math::
2122
\frac{1}{2}\, \lambda\, \Delta \mathbf{q}^\top M(\mathbf{q})\,\Delta \mathbf{q},
2223
2324
where :math:`\Delta \mathbf{q}\in\mathbb{R}^{n_v}` is the vector of joint
24-
displacements, :math:`M(\mathbf{q})` is the joint-space inertia matrix
25-
(dependent on the current configuration), and :math:`\lambda` is the scalar
26-
strength of the regularization.
25+
displacements, :math:`M(\mathbf{q})` is the joint-space inertia matrix, and
26+
:math:`\lambda` is the scalar strength of the regularization.
2727
2828
.. note::
2929
30-
This task penalizes DoFs in proportion to their joint-space inertia, so
31-
higher-inertia (i.e., heavier) links will move less.
30+
This task can be seen as an inertia-weighted version of the
31+
:class:`~.DampingTask`. Degrees of freedom with higher inertia will move less
32+
for the same cost.
3233
3334
.. warning::
3435

mink/tasks/posture_task.py

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -15,10 +15,10 @@
1515

1616

1717
class PostureTask(Task):
18-
"""Regulate the joint angles of the robot towards a desired posture.
18+
"""Regulate joint angles towards a target posture.
1919
20-
Using this task with a low cost value is useful as a regularizer when the problem
21-
is under-constrained.
20+
Often used with a low priority in the task stack, this task acts like a regularizer,
21+
biasing the solution toward a specific joint configuration.
2222
2323
Attributes:
2424
target_q: Target configuration :math:`q^*`, of shape :math:`(n_q,)`. Units are
@@ -83,7 +83,8 @@ def set_target(self, target_q: npt.ArrayLike) -> None:
8383
"""Set the target posture.
8484
8585
Args:
86-
target_q: Desired joint configuration.
86+
target_q: A vector of shape (nq,) representing the desired joint
87+
configuration.
8788
"""
8889
target_q = np.atleast_1d(target_q)
8990
if target_q.ndim != 1 or target_q.shape[0] != (self.nq):
@@ -94,7 +95,7 @@ def set_target(self, target_q: npt.ArrayLike) -> None:
9495
self.target_q = target_q.copy()
9596

9697
def set_target_from_configuration(self, configuration: Configuration) -> None:
97-
"""Set the target posture from the current configuration.
98+
"""Set the target posture by extracting it from the current configuration.
9899
99100
Args:
100101
configuration: Robot configuration :math:`q`.

0 commit comments

Comments
 (0)