diff --git a/pink/barriers/exceptions.py b/pink/barriers/exceptions.py index 03836e0..43f0d14 100644 --- a/pink/barriers/exceptions.py +++ b/pink/barriers/exceptions.py @@ -4,6 +4,8 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright 2024 Ivan Domrachev, Simeon Nedelchev +"""Exceptions raised by barriers.""" + from ..exceptions import PinkError diff --git a/pink/barriers/self_collision_barrier.py b/pink/barriers/self_collision_barrier.py index 2711db4..eac628c 100644 --- a/pink/barriers/self_collision_barrier.py +++ b/pink/barriers/self_collision_barrier.py @@ -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 @@ -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. @@ -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. @@ -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`. @@ -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( [ @@ -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: @@ -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 diff --git a/pink/configuration.py b/pink/configuration.py index 01e8b31..bf30338 100644 --- a/pink/configuration.py +++ b/pink/configuration.py @@ -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. """ @@ -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. @@ -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 diff --git a/pink/tasks/linear_holonomic_task.py b/pink/tasks/linear_holonomic_task.py index ac17c8b..60d7620 100644 --- a/pink/tasks/linear_holonomic_task.py +++ b/pink/tasks/linear_holonomic_task.py @@ -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 diff --git a/pink/utils.py b/pink/utils.py index ff9d644..c0cf25d 100644 --- a/pink/utils.py +++ b/pink/utils.py @@ -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 != "":