Skip to content

Commit

Permalink
Fix all ruff check errors
Browse files Browse the repository at this point in the history
  • Loading branch information
stephane-caron committed Aug 27, 2024
1 parent 73b138b commit 178b6a6
Show file tree
Hide file tree
Showing 5 changed files with 67 additions and 52 deletions.
2 changes: 2 additions & 0 deletions pink/barriers/exceptions.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@
# SPDX-License-Identifier: Apache-2.0
# Copyright 2024 Ivan Domrachev, Simeon Nedelchev

"""Exceptions raised by barriers."""

from ..exceptions import PinkError


Expand Down
71 changes: 39 additions & 32 deletions pink/barriers/self_collision_barrier.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
# SPDX-License-Identifier: Apache-2.0
# Copyright 2024 Ivan Domrachev, Simeon Nedelchev

"""Self Collision Avoidance Barrier with hpp-fcl"""
"""Self Collision Avoidance Barrier with hpp-fcl."""

from typing import Union

Expand All @@ -19,24 +19,26 @@
class SelfCollisionBarrier(Barrier):
r"""Self Collision Avoidance Barrier.
This class defines a barrier function based on the smooth convex collision geometry.
using hpp-fcl through Pinocchio. Note that for non-smooth collision geometries
behaviour is undefined.
This class defines a barrier function based on the smooth convex collision
geometry. using hpp-fcl through Pinocchio. Note that for non-smooth
collision geometries behaviour is undefined.
The barrier function is defined as:
.. math::
h(q) = \begin{bmatrix} \ldots \\ d(p^1_i, p^2_i) - d_{min}^2 \\ \ldots \end{bmatrix}
\quad \forall i \in 0 \ldots N
h(q) = \begin{bmatrix} \ldots \\ d(p^1_i, p^2_i) - d_{min}^2 \\
\ldots \end{bmatrix} \quad \forall i \in 0 \ldots N
where :math:`N` is number of collision pairs, :math:`p^k_i` is the :math:`k-th` body in :math:`i`-th collision pair,
:math:`d(p^1_i, p^2_i)` is the distance between collision bodies in the pair,
and :math:`d_{min}` is minimal distance between any collision bodies.
where :math:`N` is number of collision pairs, :math:`p^k_i` is the
:math:`k-th` body in :math:`i`-th collision pair, :math:`d(p^1_i, p^2_i)`
is the distance between collision bodies in the pair, and :math:`d_{min}`
is minimal distance between any collision bodies.
Note:
The number of evaluated collision pairs might not be equal to the number of all collision pairs.
If the former is lower, then only the closest collision pairs will be considered.
The number of evaluated collision pairs might not be equal to the
number of all collision pairs. If the former is lower, then only the
closest collision pairs will be considered.
Attributes:
d_min: Minimum distance between collision pairs.
Expand All @@ -55,9 +57,9 @@ def __init__(
Args:
n_collision_pairs: Number of collision pairs.
Note that the number of collision pairs doesn't have to be equal to
the total number of collision pairs in the model. If it is less,
than only the closest collision pairs will be used.
Note that the number of collision pairs doesn't have to be
equal to the total number of collision pairs in the model. If
it is less, than only the closest collision pairs will be used.
gain: Barrier gain. Defaults to 1.0.
safe_displacement_gain: gain for the safe backup displacement.
cost term. Defaults to 1.0.
Expand Down Expand Up @@ -85,18 +87,18 @@ def __init__(
def compute_barrier(self, configuration: Configuration) -> np.ndarray:
r"""Compute the value of the barrier function.
The barrier function is computed as the vector of lowest distances between collision pairs.
The barrier function is defined as:
The barrier function is computed as the vector of lowest distances
between collision pairs. It is defined as:
.. math::
h(q) = \begin{bmatrix} \ldots \\ d(p^1_i, p^2_i) - d_{min}^2 \\ \ldots \end{bmatrix}
\quad \forall i \in 1 \ldots N
h(q) = \begin{bmatrix} \ldots \\ d(p^1_i, p^2_i) - d_{min}^2 \\
\ldots \end{bmatrix} \quad \forall i \in 1 \ldots N
where :math:`N` is number of collision pairs, :math:`p^k_i` is the :math:`k-th` body in :math:`i`-th collision pair,
:math:`d(p^1_i, p^2_i)` is the distance between collision bodies in the pair,
and :math:`d_{min}` is the minimal distance between any collision bodies.
where :math:`N` is number of collision pairs, :math:`p^k_i` is the
:math:`k-th` body in :math:`i`-th collision pair, :math:`d(p^1_i,
p^2_i)` is the distance between collision bodies in the pair, and
:math:`d_{min}` is the minimal distance between any collision bodies.
Args:
configuration: Robot configuration :math:`q`.
Expand All @@ -107,8 +109,9 @@ def compute_barrier(self, configuration: Configuration) -> np.ndarray:
"""
if len(configuration.collision_model.collisionPairs) < self.dim:
raise InvalidCollisionPairs(
f"The number of collision pairs ({len(configuration.collision_model.collisionPairs)}) is less "
f"than the barrier dimension ({self.dim})."
"The number of collision pairs "
f"({len(configuration.collision_model.collisionPairs)}) "
f"is less than the barrier dimension ({self.dim})."
)
distances = np.array(
[
Expand All @@ -129,17 +132,21 @@ def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
r"""Compute the Jacobian matrix of the barrier function.
The Jacobian matrix could be represented as stacked gradients of each
collision pair distance function w.r.t. joints. They are computed based on
frames Jacobians and normal surface vector at nearest distance points n.
collision pair distance function w.r.t. joints. They are computed based
on frames Jacobians and normal surface vector at nearest distance
points n.
The gradient, a.k.a i-th row in the Jacobian matrix, is given by:
The gradient, *a.k.a.* the i-th row in the Jacobian matrix, is given
by:
.. math::
J_i = n_1^T J^1_p + (r_1 \times n_1)^T J^1_w + n_2^T J^2_p + (r_2 \times n_2)^T J^2_w,
J_i = n_1^T J^1_p + (r_1 \times n_1)^T J^1_w +
n_2^T J^2_p + (r_2 \times n_2)^T J^2_w,
where :math:`n_{1,2}` are normal vectors (note that :math:`n_1 = -n_2`), :math:`r_{1, 2}` are vectors from
frame origin and nearest point, :math:`J^{1, 2}_{p, w}` are position/orientation Jacobians of
where :math:`n_{1,2}` are normal vectors (note that :math:`n_1 =
-n_2`), :math:`r_{1, 2}` are vectors from frame origin and nearest
point, :math:`J^{1, 2}_{p, w}` are position/orientation Jacobians of
respective frame.
Args:
Expand Down Expand Up @@ -212,8 +219,8 @@ def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
J[i] = Jrow_v.copy()

# If collision is undefined, or during the collision, some values
# might be nans. In this case, set them to zero.
# Note that for non-colliding smooth convex functions no nans are present.
# might be nans. In this case, set them to zero. Note that for
# non-colliding smooth convex functions no nans are present.
J = np.nan_to_num(J)

return J
36 changes: 20 additions & 16 deletions pink/configuration.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,29 +30,31 @@ class Configuration:
We rely on typing to make sure the proper forward kinematics functions have
been called beforehand. In Pinocchio, these functions are:
- ``pin.computeJointJacobians(model, data, configuration)``
- ``pin.updateFramePlacements(model, data)``
.. code:: python
pin.computeJointJacobians(model, data, configuration)
pin.updateFramePlacements(model, data)
The former computes the full model Jacobian into ``data.J``. (It also
computes forward kinematics, so there is no need to further call
``pin.forwardKinematics(model, data, configuration)``.) The latter updates
frame placements.
Additionally, if collision model is provided, it is used to evaluate
distances between frames using following functions:
Additionally, if a collision model is provided, it is used to evaluate
distances between frames by calling the following two functions:
- ``pin.computeCollisions(model, data, collision_model, collision_data, q)`` # noqa: E501
- ``pin.updateGeometryPlacements(model, data, collision_model, collision_data, q)`` # noqa: E501
.. code:: python
Notes:
This class is meant to be used as a subclass of pin.RobotWrapper, not
wrap it. However, right now pin.RobotWrapper does not have a shallow
copy constructor. TODO(scaron): bring it up upstream.
pin.computeCollisions(
model, data, collision_model, collision_data, q)
pin.updateGeometryPlacements(
model, data, collision_model, collision_data, q)
Attributes:
data: Data corresponding to :data:`Configuration.model`.
model: Kinodynamic model.
collision_data: Data corresponding to :data:`Configuration.collision_model`.
collision_data: Data corresponding to
:data:`Configuration.collision_model`.
collision_model: Collision model.
q: Configuration vector for the robot model.
"""
Expand Down Expand Up @@ -125,8 +127,10 @@ def __init__(
self.update(None)

def update(self, q: Optional[np.ndarray] = None) -> None:
"""Update configuration to a new vector and run forward kinematics and
collision pairs distance calculations, if specified.
"""Update configuration to a new vector.
Calling this function runs forward kinematics and computes
collision-pair distances, if applicable.
Args:
q: New configuration vector.
Expand Down Expand Up @@ -164,9 +168,9 @@ def check_limits(
Args:
tol: Tolerance in radians.
safe_break (bool): If True, stop execution and raise an exception
if the current configuration is outside limits. If False, print
a warning and continue execution.
safety_break: If True, stop execution and raise an exception if the
current configuration is outside limits. If False, print a
warning and continue execution.
Raises:
NotWithinConfigurationLimits: If the current configuration is
Expand Down
2 changes: 1 addition & 1 deletion pink/tasks/linear_holonomic_task.py
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ class LinearHolonomicTask(Task):
where :math:`\mathfrak{g}^\ell_{q_0}` is the Lie algebra associated
with the Lie group :math:`\mathcal{C}` (our configuration space), taken
in the local frame (:math:`\ell`, *a.k.a* body frame) at the reference
in the local frame (:math:`\ell`, *a.k.a.* body frame) at the reference
configuration :math:`q_0`.
We take the *local* difference :math:`q \ominus^\ell q_0` between our
Expand Down
8 changes: 5 additions & 3 deletions pink/utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -122,11 +122,13 @@ def process_collision_pairs(
Args:
model: robot model.
collision_model: Collision model of the robot.
srdf_path: Path to the SRDF file, which used to exclude collision pairs.
Defaults to empty string, meaning no collision pairs are excluded.
srdf_path: Path to the SRDF file, which used to exclude collision
pairs. Defaults to empty string, meaning no collision pairs are
excluded.
Returns:
collision_data: Collision data, generated after updating collision_model.
collision_data: Collision data, generated after updating
collision_model.
"""
collision_model.addAllCollisionPairs()
if srdf_path != "":
Expand Down

0 comments on commit 178b6a6

Please sign in to comment.