Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[WIP] #76 Add General Spherical joint #85

Merged
merged 11 commits into from
Sep 13, 2023
Merged

[WIP] #76 Add General Spherical joint #85

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
43ed374
Adding the GeneralSpherical joint into joint numpy
ANaaim Sep 1, 2023
a8b107b
GeneralSpherical joint seems functional
ANaaim Sep 1, 2023
a29554f
Blacked
ANaaim Sep 1, 2023
e47fbce
Fusion of General_Spherical into Spherical
ANaaim Sep 11, 2023
32fc29b
We remove the GeneralSpherical Joint
ANaaim Sep 11, 2023
632e16b
Blacked
ANaaim Sep 11, 2023
03c5174
Modification to respect BaseJoint variable and Natural marker variable
ANaaim Sep 11, 2023
50bc1fb
Squeeze the constraint + transfer the name of marker to casadi + Blacked
ANaaim Sep 12, 2023
6aac307
V1 new_test
ANaaim Sep 12, 2023
0f60fcf
Blacked
ANaaim Sep 13, 2023
9df9fa9
Correction to better differentiate the two type of spherical joint
ANaaim Sep 13, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 32 additions & 3 deletions bionc/bionc_casadi/joint.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
from .natural_segment import NaturalSegment
from .natural_coordinates import SegmentNaturalCoordinates
from .natural_velocities import SegmentNaturalVelocities
from .natural_marker import NaturalMarker
from ..protocols.joint import JointBase
from .natural_vector import NaturalVector
from ..utils.enums import NaturalAxis, CartesianAxis, EulerSequence, TransformationMatrixType
Expand Down Expand Up @@ -331,6 +332,8 @@ def __init__(
parent: NaturalSegment,
child: NaturalSegment,
index: int,
parent_point: str = None,
child_point: str = None,
projection_basis: EulerSequence = None,
parent_basis: TransformationMatrixType = None,
child_basis: TransformationMatrixType = None,
Expand All @@ -339,6 +342,29 @@ def __init__(
name, parent, child, index, projection_basis, parent_basis, child_basis
)
self.nb_constraints = 3
self.parent_point = (
NaturalMarker(
name=f"{self.name}_parent_point",
parent_name=self.parent.name,
position=NaturalVector.distal(),
is_technical=False,
is_anatomical=True,
)
if parent_point is None
else parent.marker_from_name(parent_point)
)

self.child_point = (
NaturalMarker(
name=f"{self.name}_child_point",
parent_name=self.child.name,
position=NaturalVector.proximal(),
is_technical=False,
is_anatomical=True,
)
if child_point is None
else child.marker_from_name(child_point)
)

def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> MX:
"""
Expand All @@ -350,23 +376,26 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura
MX
Kinematic constraints of the joint [3, 1]
"""
constraint = Q_parent.rd - Q_child.rp
parent_point_location = self.parent_point.position_in_global(Q_parent)
child_point_location = self.child_point.position_in_global(Q_child)

constraint = parent_point_location - child_point_location

return constraint

def parent_constraint_jacobian(
self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates
) -> MX:
K_k_parent = MX.zeros((self.nb_constraints, 12))
K_k_parent[:3, 6:9] = MX.eye(3)
K_k_parent[:3, :] = self.parent_point.interpolation_matrix

return K_k_parent

def child_constraint_jacobian(
self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates
) -> MX:
K_k_child = MX.zeros((self.nb_constraints, 12))
K_k_child[:3, 3:6] = -MX.eye(3)
K_k_child[:3, :] = -self.child_point.interpolation_matrix

return K_k_child

Expand Down
42 changes: 38 additions & 4 deletions bionc/bionc_numpy/joint.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
from .natural_segment import NaturalSegment
from .natural_coordinates import SegmentNaturalCoordinates
from .natural_velocities import SegmentNaturalVelocities
from .natural_marker import NaturalMarker
from ..protocols.joint import JointBase
from ..utils.enums import NaturalAxis, CartesianAxis, EulerSequence, TransformationMatrixType
from .natural_vector import NaturalVector
Expand Down Expand Up @@ -375,6 +376,8 @@ def __init__(
parent: NaturalSegment,
child: NaturalSegment,
index: int,
parent_point: str = None,
child_point: str = None,
projection_basis: EulerSequence = None,
parent_basis: TransformationMatrixType = None,
child_basis: TransformationMatrixType = None,
Expand All @@ -383,6 +386,32 @@ def __init__(
name, parent, child, index, projection_basis, parent_basis, child_basis, None
)
self.nb_constraints = 3
self.parent_point_str = parent_point
self.child_point_str = child_point # to transfer to casadi later on

self.parent_point = (
NaturalMarker(
name=f"{self.name}_parent_point",
parent_name=self.parent.name,
position=NaturalVector.distal(),
is_technical=False,
is_anatomical=True,
)
if parent_point is None
else parent.marker_from_name(parent_point)
)

self.child_point = (
NaturalMarker(
name=f"{self.name}_child_point",
parent_name=self.child.name,
position=NaturalVector.proximal(),
is_technical=False,
is_anatomical=True,
)
if child_point is None
else child.marker_from_name(child_point)
)

def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> np.ndarray:
"""
Expand All @@ -394,23 +423,26 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura
np.ndarray
Kinematic constraints of the joint [3, 1]
"""
constraint = Q_parent.rd - Q_child.rp
parent_point_location = self.parent_point.position_in_global(Q_parent)
child_point_location = self.child_point.position_in_global(Q_child)

return constraint
constraint = parent_point_location - child_point_location

return constraint.squeeze()

def parent_constraint_jacobian(
self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates
) -> np.ndarray:
K_k_parent = np.zeros((self.nb_constraints, 12))
K_k_parent[:3, 6:9] = np.eye(3)
K_k_parent[:3, :] = self.parent_point.interpolation_matrix

return K_k_parent

def child_constraint_jacobian(
self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates
) -> np.ndarray:
K_k_child = np.zeros((self.nb_constraints, 12))
K_k_child[:3, 3:6] = -np.eye(3)
K_k_child[:3, :] = -self.child_point.interpolation_matrix

return K_k_child

Expand Down Expand Up @@ -474,6 +506,8 @@ def to_mx(self):
parent=self.parent.to_mx(),
child=self.child.to_mx(),
index=self.index,
parent_point=self.parent_point_str,
child_point=self.child_point_str,
projection_basis=self.projection_basis,
parent_basis=self.parent_basis,
child_basis=self.child_basis,
Expand Down
113 changes: 105 additions & 8 deletions tests/test_joints.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
import numpy as np
import pytest

from bionc import JointType, NaturalAxis, CartesianAxis
from bionc import JointType, NaturalAxis, CartesianAxis, NaturalMarker
from .utils import TestUtils


Expand Down Expand Up @@ -78,14 +78,41 @@ def test_joints(bionc_type, joint_type: JointType):
assert joint.nb_joint_dof == 2
assert joint.nb_constraints == 4
elif joint_type == JointType.SPHERICAL:
joint = Joint.Spherical(
name="spherical",
# Spherical joint can be defined in two different ways:
# 1. The default with no option consist in using the rd point for the parent and rp child segment
# 2. By specifying the joint location in the parent and child segment

box.add_natural_marker_from_segment_coordinates(
name="P1",
location=[0.1, 0.2, 0.3],
is_anatomical=True,
)
bbox.add_natural_marker_from_segment_coordinates(
name="P2",
location=[0.2, 0.04, 0.05],
is_anatomical=True,
)

joint_distal_proximal = Joint.Spherical(
name="joint_distal_proximal",
parent=box,
child=bbox,
index=0,
)
assert joint.nb_joint_dof == 3
assert joint.nb_constraints == 3
assert joint_distal_proximal.nb_joint_dof == 3
assert joint_distal_proximal.nb_constraints == 3

joint_custom_location = Joint.Spherical(
name="joint_custom_location",
parent=box,
child=bbox,
index=1,
parent_point="P1",
child_point="P2",
)
assert joint_custom_location.nb_joint_dof == 3
assert joint_custom_location.nb_constraints == 3

elif joint_type == JointType.GROUND_REVOLUTE:
joint = GroundJoint.Hinge(
name="hinge",
Expand Down Expand Up @@ -328,12 +355,13 @@ def test_joints(bionc_type, joint_type: JointType):
)

elif joint_type == JointType.SPHERICAL:
## Spherical joint rd rp
TestUtils.assert_equal(
joint.constraint(Q1, Q2),
joint_distal_proximal.constraint(Q1, Q2),
np.array([-0.3, 0.9, 0.9]),
decimal=6,
)
parent_jacobian, child_jacobian = joint.constraint_jacobian(Q1, Q2)
parent_jacobian, child_jacobian = joint_distal_proximal.constraint_jacobian(Q1, Q2)
TestUtils.assert_equal(
parent_jacobian,
np.array(
Expand All @@ -357,7 +385,7 @@ def test_joints(bionc_type, joint_type: JointType):
decimal=6,
)

parent_jacobian_dot, child_jacobian_dot = joint.constraint_jacobian_derivative(Q1, Q2)
parent_jacobian_dot, child_jacobian_dot = joint_distal_proximal.constraint_jacobian_derivative(Q1, Q2)
TestUtils.assert_equal(
parent_jacobian_dot,
np.zeros((3, 12)),
Expand All @@ -369,6 +397,75 @@ def test_joints(bionc_type, joint_type: JointType):
decimal=6,
)

TestUtils.assert_equal(
joint_custom_location.constraint(Q1, Q2),
np.array([-0.227081, 0.071451, 0.849838]),
decimal=6,
)
parent_jacobian, child_jacobian = joint_custom_location.constraint_jacobian(Q1, Q2)

TestUtils.assert_equal(
parent_jacobian,
np.array(
[
[0.1, 0.0, 0.0, 1.2, 0.0, 0.0, -0.2, 0.0, 0.0, 0.3, 0.0, 0.0],
[0.0, 0.1, 0.0, 0.0, 1.2, 0.0, 0.0, -0.2, 0.0, 0.0, 0.3, 0.0],
[0.0, 0.0, 0.1, 0.0, 0.0, 1.2, 0.0, 0.0, -0.2, 0.0, 0.0, 0.3],
]
),
decimal=6,
)
TestUtils.assert_equal(
child_jacobian,
np.array(
[
[
-0.2,
0.0,
0.0,
-1.0117535131559996,
0.0,
0.0,
0.01175351315599963,
0.0,
0.0,
-0.0107387974365452,
0.0,
0.0,
],
[
0.0,
-0.2,
0.0,
-0.0,
-1.0117535131559996,
0.0,
0.0,
0.01175351315599963,
0.0,
0.0,
-0.0107387974365452,
0.0,
],
[
0.0,
0.0,
-0.2,
0.0,
0.0,
-1.0117535131559996,
0.0,
0.0,
0.01175351315599963,
0.0,
0.0,
-0.0107387974365452,
],
]
),
decimal=6,
)

elif joint_type == JointType.CONSTANT_LENGTH:
TestUtils.assert_equal(
joint.constraint(Q1, Q2),
Expand Down
Loading