Skip to content

Commit

Permalink
Merge branch 'main' into main
Browse files Browse the repository at this point in the history
  • Loading branch information
ConnorTingley authored Jan 15, 2025
2 parents fbb34e7 + 1d25f1a commit 1ba76da
Show file tree
Hide file tree
Showing 6 changed files with 25 additions and 26 deletions.
23 changes: 11 additions & 12 deletions src/jaxsim/math/adjoint.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,13 @@ def from_quaternion_and_translation(
Create an adjoint matrix from a quaternion and a translation.
Args:
quaternion (jtp.Vector): A quaternion vector (4D) representing orientation. Default is [1, 0, 0, 0].
translation (jtp.Vector): A translation vector (3D). Default is [0, 0, 0].
inverse (bool): Whether to compute the inverse adjoint. Default is False.
normalize_quaternion (bool): Whether to normalize the quaternion before creating the adjoint.
Default is False.
quaternion: A quaternion vector (4D) representing orientation.
translation: A translation vector (3D).
inverse: Whether to compute the inverse adjoint.
normalize_quaternion: Whether to normalize the quaternion before creating the adjoint.
Returns:
jtp.Matrix: The adjoint matrix.
The adjoint matrix.
"""
quaternion = quaternion if quaternion is not None else jnp.array([1.0, 0, 0, 0])
translation = translation if translation is not None else jnp.zeros(3)
Expand Down Expand Up @@ -74,12 +73,12 @@ def from_rotation_and_translation(
Create an adjoint matrix from a rotation matrix and a translation vector.
Args:
rotation (jtp.Matrix): A 3x3 rotation matrix. Default is identity.
translation (jtp.Vector): A translation vector (3D). Default is [0, 0, 0].
inverse (bool): Whether to compute the inverse adjoint. Default is False.
rotation: A 3x3 rotation matrix.
translation: A translation vector (3D).
inverse: Whether to compute the inverse adjoint. Default is False.
Returns:
jtp.Matrix: The adjoint matrix.
The adjoint matrix.
"""
rotation = rotation if rotation is not None else jnp.eye(3)
translation = translation if translation is not None else jnp.zeros(3)
Expand Down Expand Up @@ -116,7 +115,7 @@ def to_transform(adjoint: jtp.Matrix) -> jtp.Matrix:
adjoint: The adjoint matrix (6x6).
Returns:
jtp.Matrix: The transformation matrix (4x4).
The transformation matrix (4x4).
"""
X = adjoint.squeeze()
assert X.shape == (6, 6)
Expand All @@ -142,7 +141,7 @@ def inverse(adjoint: jtp.Matrix) -> jtp.Matrix:
adjoint: The adjoint matrix.
Returns:
jtp.Matrix: The inverse adjoint matrix.
The inverse adjoint matrix.
"""
A_X_B = adjoint.reshape(-1, 6, 6)

Expand Down
4 changes: 2 additions & 2 deletions src/jaxsim/math/cross.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ def vx(velocity_sixd: jtp.Vector) -> jtp.Matrix:
velocity_sixd: A 6D velocity vector [v, ω].
Returns:
jtp.Matrix: The cross product matrix (6x6).
The cross product matrix (6x6).
Raises:
ValueError: If the input vector does not have a size of 6.
Expand Down Expand Up @@ -49,7 +49,7 @@ def vx_star(velocity_sixd: jtp.Vector) -> jtp.Matrix:
velocity_sixd: A 6D velocity vector [v, ω].
Returns:
jtp.Matrix: The negative transpose of the cross product matrix (6x6).
The negative transpose of the cross product matrix (6x6).
Raises:
ValueError: If the input vector does not have a size of 6.
Expand Down
4 changes: 2 additions & 2 deletions src/jaxsim/math/inertia.py
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ def to_sixd(mass: jtp.Float, com: jtp.Vector, I: jtp.Matrix) -> jtp.Matrix:
I: The 3x3 inertia matrix.
Returns:
jtp.Matrix: The 6x6 inertia matrix.
The 6x6 inertia matrix.
Raises:
ValueError: If the shape of the inertia matrix I is not (3, 3).
Expand Down Expand Up @@ -49,7 +49,7 @@ def to_params(M: jtp.Matrix) -> tuple[jtp.Float, jtp.Vector, jtp.Matrix]:
M: The 6x6 inertia matrix.
Returns:
tuple[jtp.Float, jtp.Vector, jtp.Matrix]: A tuple containing mass, center of mass (3D), and inertia matrix (3x3).
A tuple containing mass, center of mass (3D), and inertia matrix (3x3).
Raises:
ValueError: If the input matrix M has an unexpected shape.
Expand Down
10 changes: 5 additions & 5 deletions src/jaxsim/math/quaternion.py
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ def to_xyzw(wxyz: jtp.Vector) -> jtp.Vector:
wxyz: Quaternion in WXYZ representation.
Returns:
jtp.Vector: Quaternion in XYZW representation.
Quaternion in XYZW representation.
"""
return wxyz.squeeze()[jnp.array([1, 2, 3, 0])]

Expand All @@ -34,7 +34,7 @@ def to_wxyz(xyzw: jtp.Vector) -> jtp.Vector:
xyzw: Quaternion in XYZW representation.
Returns:
jtp.Vector: Quaternion in WXYZ representation.
Quaternion in WXYZ representation.
"""
return xyzw.squeeze()[jnp.array([3, 0, 1, 2])]

Expand All @@ -47,7 +47,7 @@ def to_dcm(quaternion: jtp.Vector) -> jtp.Matrix:
quaternion: Quaternion in XYZW representation.
Returns:
jtp.Matrix: Direction cosine matrix (DCM).
The Direction cosine matrix (DCM).
"""
return jaxlie.SO3(wxyz=quaternion).as_matrix()

Expand All @@ -60,7 +60,7 @@ def from_dcm(dcm: jtp.Matrix) -> jtp.Vector:
dcm: Direction cosine matrix (DCM).
Returns:
jtp.Vector: Quaternion in XYZW representation.
Quaternion in WXYZ representation.
"""
return jaxlie.SO3.from_matrix(matrix=dcm).wxyz

Expand All @@ -81,7 +81,7 @@ def derivative(
K (float): A scaling factor.
Returns:
jtp.Vector: The derivative of the quaternion.
The derivative of the quaternion.
"""
ω = omega.squeeze()
quaternion = quaternion.squeeze()
Expand Down
6 changes: 3 additions & 3 deletions src/jaxsim/math/rotation.py
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ def x(theta: jtp.Float) -> jtp.Matrix:
theta: Rotation angle in radians.
Returns:
jtp.Matrix: 3D rotation matrix.
The 3D rotation matrix.
"""

return jaxlie.SO3.from_x_radians(theta=theta).as_matrix()
Expand All @@ -35,7 +35,7 @@ def y(theta: jtp.Float) -> jtp.Matrix:
theta: Rotation angle in radians.
Returns:
jtp.Matrix: 3D rotation matrix.
The 3D rotation matrix.
"""

return jaxlie.SO3.from_y_radians(theta=theta).as_matrix()
Expand All @@ -49,7 +49,7 @@ def z(theta: jtp.Float) -> jtp.Matrix:
theta: Rotation angle in radians.
Returns:
jtp.Matrix: 3D rotation matrix.
The 3D rotation matrix.
"""

return jaxlie.SO3.from_z_radians(theta=theta).as_matrix()
Expand Down
4 changes: 2 additions & 2 deletions src/jaxsim/math/skew.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ def wedge(vector: jtp.Vector) -> jtp.Matrix:
vector: A 3D vector.
Returns:
jtp.Matrix: The skew-symmetric matrix corresponding to the input vector.
The skew-symmetric matrix corresponding to the input vector.
"""

Expand Down Expand Up @@ -45,7 +45,7 @@ def vee(matrix: jtp.Matrix) -> jtp.Vector:
matrix: A 3x3 skew-symmetric matrix.
Returns:
jtp.Vector: The 3D vector extracted from the input matrix.
The 3D vector extracted from the input matrix.
"""
vector = 0.5 * jnp.vstack(
Expand Down

0 comments on commit 1ba76da

Please sign in to comment.