From 4039df18888db94c7fed1318b12fcaf20085685e Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Wed, 4 Oct 2023 21:45:51 +0200 Subject: [PATCH] Fix format and warnings for docs --- docs/conf.py | 2 +- src/jaxsim/high_level/model.py | 3 +-- src/jaxsim/math/adjoint.py | 3 +++ src/jaxsim/math/conv.py | 3 +++ src/jaxsim/math/cross.py | 3 +++ src/jaxsim/math/inertia.py | 4 ++++ src/jaxsim/math/joint.py | 4 ++++ src/jaxsim/math/quaternion.py | 2 ++ src/jaxsim/math/rotation.py | 4 ++++ src/jaxsim/math/skew.py | 2 ++ src/jaxsim/parsers/descriptions/collision.py | 4 ++++ src/jaxsim/parsers/descriptions/joint.py | 6 ++++- src/jaxsim/parsers/descriptions/link.py | 1 + src/jaxsim/parsers/descriptions/model.py | 7 +++--- src/jaxsim/parsers/kinematic_graph.py | 1 + src/jaxsim/physics/algos/crba.py | 2 +- .../physics/algos/forward_kinematics.py | 2 +- src/jaxsim/physics/algos/soft_contacts.py | 22 +++++++++++++++---- src/jaxsim/physics/algos/terrain.py | 4 ++-- src/jaxsim/physics/model/ground_contact.py | 3 +-- src/jaxsim/simulation/simulator.py | 3 +-- 21 files changed, 66 insertions(+), 19 deletions(-) diff --git a/docs/conf.py b/docs/conf.py index 03203ab25..809ec1c28 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -1,7 +1,7 @@ # Configuration file for the Sphinx documentation builder. +import os import pathlib import sys -import os # -- Version information diff --git a/src/jaxsim/high_level/model.py b/src/jaxsim/high_level/model.py index ea37e9319..26125587d 100644 --- a/src/jaxsim/high_level/model.py +++ b/src/jaxsim/high_level/model.py @@ -137,8 +137,7 @@ def build_from_model_description( Build a Model object from a model description. Args: - model_description: A path to an SDF/URDF file, a string containing its content, - or a pre-parsed/pre-built rod model. + model_description: A path to an SDF/URDF file, a string containing its content, or a pre-parsed/pre-built rod model. model_name: The optional name of the model that overrides the one in the description. vel_repr: The velocity representation to use. gravity: The 3D gravity vector. diff --git a/src/jaxsim/math/adjoint.py b/src/jaxsim/math/adjoint.py index 98b5cbf11..62674dc66 100644 --- a/src/jaxsim/math/adjoint.py +++ b/src/jaxsim/math/adjoint.py @@ -1,9 +1,12 @@ import jax.numpy as jnp + import jaxsim.typing as jtp from jaxsim.sixd import so3 + from .quaternion import Quaternion from .skew import Skew + class Adjoint: @staticmethod def from_quaternion_and_translation( diff --git a/src/jaxsim/math/conv.py b/src/jaxsim/math/conv.py index c9e6200c5..4db6a790a 100644 --- a/src/jaxsim/math/conv.py +++ b/src/jaxsim/math/conv.py @@ -1,7 +1,10 @@ import jax.numpy as jnp + import jaxsim.typing as jtp + from .skew import Skew + class Convert: @staticmethod def coordinates_tf(X: jtp.Matrix, p: jtp.Matrix) -> jtp.Matrix: diff --git a/src/jaxsim/math/cross.py b/src/jaxsim/math/cross.py index 55c5d95fd..daa006fb0 100644 --- a/src/jaxsim/math/cross.py +++ b/src/jaxsim/math/cross.py @@ -1,7 +1,10 @@ import jax.numpy as jnp + import jaxsim.typing as jtp + from .skew import Skew + class Cross: @staticmethod def vx(velocity_sixd: jtp.Vector) -> jtp.Matrix: diff --git a/src/jaxsim/math/inertia.py b/src/jaxsim/math/inertia.py index 6301baa98..c4b8f118d 100644 --- a/src/jaxsim/math/inertia.py +++ b/src/jaxsim/math/inertia.py @@ -1,8 +1,12 @@ from typing import Tuple + import jax.numpy as jnp + import jaxsim.typing as jtp + from .skew import Skew + class Inertia: @staticmethod def to_sixd(mass: jtp.Float, com: jtp.Vector, I: jtp.Matrix) -> jtp.Matrix: diff --git a/src/jaxsim/math/joint.py b/src/jaxsim/math/joint.py index 5550ed9d4..f52182436 100644 --- a/src/jaxsim/math/joint.py +++ b/src/jaxsim/math/joint.py @@ -1,10 +1,14 @@ from typing import Tuple, Union + import jax.numpy as jnp + import jaxsim.typing as jtp from jaxsim.parsers.descriptions import JointDescriptor, JointGenericAxis, JointType + from .adjoint import Adjoint from .rotation import Rotation + def jcalc( jtyp: Union[JointType, JointDescriptor], q: jtp.Float ) -> Tuple[jtp.Matrix, jtp.Vector]: diff --git a/src/jaxsim/math/quaternion.py b/src/jaxsim/math/quaternion.py index 3f26058aa..f03b9c107 100644 --- a/src/jaxsim/math/quaternion.py +++ b/src/jaxsim/math/quaternion.py @@ -1,8 +1,10 @@ import jax.lax import jax.numpy as jnp + import jaxsim.typing as jtp from jaxsim.sixd import so3 + class Quaternion: @staticmethod def to_xyzw(wxyz: jtp.Vector) -> jtp.Vector: diff --git a/src/jaxsim/math/rotation.py b/src/jaxsim/math/rotation.py index 150139da1..532615029 100644 --- a/src/jaxsim/math/rotation.py +++ b/src/jaxsim/math/rotation.py @@ -1,10 +1,14 @@ from typing import Tuple + import jax import jax.numpy as jnp + import jaxsim.typing as jtp from jaxsim.sixd import so3 + from .skew import Skew + class Rotation: @staticmethod def x(theta: jtp.Float) -> jtp.Matrix: diff --git a/src/jaxsim/math/skew.py b/src/jaxsim/math/skew.py index 07590627a..2d0c2d114 100644 --- a/src/jaxsim/math/skew.py +++ b/src/jaxsim/math/skew.py @@ -1,6 +1,8 @@ import jax.numpy as jnp + import jaxsim.typing as jtp + class Skew: """ A utility class for skew-symmetric matrix operations. diff --git a/src/jaxsim/parsers/descriptions/collision.py b/src/jaxsim/parsers/descriptions/collision.py index 3d27a0885..b49722907 100644 --- a/src/jaxsim/parsers/descriptions/collision.py +++ b/src/jaxsim/parsers/descriptions/collision.py @@ -10,6 +10,7 @@ from .link import LinkDescription + @dataclasses.dataclass class CollidablePoint: """ @@ -58,6 +59,7 @@ def __str__(self): + ")" ) + @dataclasses.dataclass class CollisionShape(abc.ABC): """ @@ -78,6 +80,7 @@ def __str__(self): + "\n])" ) + @dataclasses.dataclass class BoxCollision(CollisionShape): """ @@ -90,6 +93,7 @@ class BoxCollision(CollisionShape): center: npt.NDArray + @dataclasses.dataclass class SphereCollision(CollisionShape): """ diff --git a/src/jaxsim/parsers/descriptions/joint.py b/src/jaxsim/parsers/descriptions/joint.py index ee8a89d36..6244e9024 100644 --- a/src/jaxsim/parsers/descriptions/joint.py +++ b/src/jaxsim/parsers/descriptions/joint.py @@ -7,6 +7,7 @@ from .link import LinkDescription + class JointType(enum.IntEnum): """ Enumeration of joint types for robot joints. @@ -31,12 +32,13 @@ class JointType(enum.IntEnum): Rx = enum.auto() Ry = enum.auto() Rz = enum.auto() - + # Prismatic joints, single axis Px = enum.auto() Py = enum.auto() Pz = enum.auto() + @dataclasses.dataclass class JointDescriptor: """ @@ -52,6 +54,7 @@ class JointDescriptor: def __hash__(self) -> int: return hash(self.__repr__()) + @dataclasses.dataclass class JointGenericAxis(JointDescriptor): """ @@ -74,6 +77,7 @@ def __eq__(self, other): def __hash__(self) -> int: return hash(self.__repr__()) + @dataclasses.dataclass class JointDescription: """ diff --git a/src/jaxsim/parsers/descriptions/link.py b/src/jaxsim/parsers/descriptions/link.py index 47b75edb0..e6a86897f 100644 --- a/src/jaxsim/parsers/descriptions/link.py +++ b/src/jaxsim/parsers/descriptions/link.py @@ -10,6 +10,7 @@ from jaxsim.sixd import se3 from jaxsim.utils import JaxsimDataclass + @jax_dataclasses.pytree_dataclass class LinkDescription(JaxsimDataclass): """ diff --git a/src/jaxsim/parsers/descriptions/model.py b/src/jaxsim/parsers/descriptions/model.py index 7ee3678d6..3421e4131 100644 --- a/src/jaxsim/parsers/descriptions/model.py +++ b/src/jaxsim/parsers/descriptions/model.py @@ -3,12 +3,15 @@ from typing import List import numpy.typing as npt + import jaxsim.logging as logging + from ..kinematic_graph import KinematicGraph, RootPose from .collision import CollidablePoint, CollisionShape from .joint import JointDescription from .link import LinkDescription + @dataclasses.dataclass(frozen=True) class ModelDescription(KinematicGraph): """ @@ -30,7 +33,6 @@ class ModelDescription(KinematicGraph): update_collision_shape_of_link(...): Enable or disable collision shapes associated with a link. collision_shape_of_link(...): Get the collision shape associated with a specific link. all_enabled_collidable_points(...): Get all enabled collidable points in the model. - """ name: str = None @@ -66,7 +68,6 @@ def build_model_from( Raises: ValueError: If invalid or missing input data. - """ # Create the full kinematic graph @@ -164,8 +165,8 @@ def reduce(self, considered_joints: List[str]) -> "ModelDescription": Raises: ValueError: If the specified joints are not part of the model. - """ + msg = "The model reduction logic assumes that removed joints have zero angles" logging.info(msg=msg) diff --git a/src/jaxsim/parsers/kinematic_graph.py b/src/jaxsim/parsers/kinematic_graph.py index b7f276eb0..491dfa846 100644 --- a/src/jaxsim/parsers/kinematic_graph.py +++ b/src/jaxsim/parsers/kinematic_graph.py @@ -29,6 +29,7 @@ class RootPose(NamedTuple): root_position (npt.NDArray): A NumPy array of shape (3,) representing the root's position. root_quaternion (npt.NDArray): A NumPy array of shape (4,) representing the root's quaternion. """ + root_position: npt.NDArray = np.zeros(3) root_quaternion: npt.NDArray = np.array([1.0, 0, 0, 0]) diff --git a/src/jaxsim/physics/algos/crba.py b/src/jaxsim/physics/algos/crba.py index c57673d88..f175e15fb 100644 --- a/src/jaxsim/physics/algos/crba.py +++ b/src/jaxsim/physics/algos/crba.py @@ -21,7 +21,7 @@ def crba(model: PhysicsModel, q: jtp.Vector) -> jtp.Matrix: Returns: jtp.Matrix: The Composite Rigid-Body Inertia Matrix (CRBA) of the articulated body or robot. """ - + _, q, _, _, _, _ = utils.process_inputs( physics_model=model, xfb=None, q=q, qd=None, tau=None, f_ext=None ) diff --git a/src/jaxsim/physics/algos/forward_kinematics.py b/src/jaxsim/physics/algos/forward_kinematics.py index b370c094e..5573116b4 100644 --- a/src/jaxsim/physics/algos/forward_kinematics.py +++ b/src/jaxsim/physics/algos/forward_kinematics.py @@ -25,7 +25,7 @@ def forward_kinematics_model( Returns: jtp.Array: A 3D array containing the forward kinematics transformations for all links. """ - + x_fb, q, _, _, _, _ = utils.process_inputs( physics_model=model, xfb=xfb, q=q, qd=None, tau=None, f_ext=None ) diff --git a/src/jaxsim/physics/algos/soft_contacts.py b/src/jaxsim/physics/algos/soft_contacts.py index 4bb341ff9..434692d93 100644 --- a/src/jaxsim/physics/algos/soft_contacts.py +++ b/src/jaxsim/physics/algos/soft_contacts.py @@ -20,7 +20,22 @@ @jax_dataclasses.pytree_dataclass class SoftContactsState: - """State of the soft contacts model.""" + """ + State of the soft contacts model. + + Attributes: + tangential_deformation (jtp.Matrix): The tangential deformation of the material at each collidable point. + + Methods: + zero(physics_model: "jaxsim.physics.model.physics_model.PhysicsModel") -> SoftContactsState: + Modify the SoftContactsState instance imposing zero tangential deformation. + + valid(physics_model: "jaxsim.physics.model.physics_model.PhysicsModel") -> bool: + Check if the soft contacts state has valid shape. + + replace(validate: bool = True, kwargs) -> SoftContactsState: + Replace attributes of the soft contacts state. + """ tangential_deformation: jtp.Matrix @@ -37,7 +52,7 @@ def zero( Returns: SoftContactsState: A SoftContactsState instance with zero tangential deformation. """ - + return SoftContactsState( tangential_deformation=jnp.zeros(shape=(3, physics_model.gc.body.size)) ) @@ -54,7 +69,7 @@ def valid( Returns: bool: True if the state has a valid shape, otherwise False. """ - + from jaxsim.simulation.utils import check_valid_shape return check_valid_shape( @@ -70,7 +85,6 @@ def replace(self, validate: bool = True, **kwargs) -> "SoftContactsState": Args: validate (bool, optional): Whether to validate the state after replacement. Defaults to True. - **kwargs: Keyword arguments for attribute replacement. Returns: SoftContactsState: A new SoftContactsState instance with replaced attributes. diff --git a/src/jaxsim/physics/algos/terrain.py b/src/jaxsim/physics/algos/terrain.py index 90ca5cd38..f8072b4eb 100644 --- a/src/jaxsim/physics/algos/terrain.py +++ b/src/jaxsim/physics/algos/terrain.py @@ -75,6 +75,6 @@ def height(self, x: float, y: float) -> float: Returns: float: The height of the terrain at the specified location on the plane. """ - + a, b, c = self.plane_normal - return -(a * x + b * x) / c \ No newline at end of file + return -(a * x + b * x) / c diff --git a/src/jaxsim/physics/model/ground_contact.py b/src/jaxsim/physics/model/ground_contact.py index 04a3be750..7e30cef63 100644 --- a/src/jaxsim/physics/model/ground_contact.py +++ b/src/jaxsim/physics/model/ground_contact.py @@ -19,8 +19,7 @@ class GroundContact: Attributes: point (npt.NDArray): An array of shape (3, N) representing the 3D positions of collidable points. - body (Static[npt.NDArray]): An array of integers representing the indices of the bodies (links) - associated with each collidable point. + body (Static[npt.NDArray]): An array of integers representing the indices of the bodies (links) associated with each collidable point. Methods: build_from(model_description: ModelDescription) -> GroundContact: diff --git a/src/jaxsim/simulation/simulator.py b/src/jaxsim/simulation/simulator.py index e32389475..df412ac76 100644 --- a/src/jaxsim/simulation/simulator.py +++ b/src/jaxsim/simulation/simulator.py @@ -235,8 +235,7 @@ def insert_model_from_description( Insert a model from a model description. Args: - model_description: A path to an SDF/URDF file, a string containing its content, - or a pre-parsed/pre-built rod model. + model_description: A path to an SDF/URDF file, a string containing its content, or a pre-parsed/pre-built rod model. model_name: The optional name of the model that overrides the one in the description. considered_joints: Optional list of joints to consider. It is also useful to specify the joint serialization.