Skip to content

Commit

Permalink
Merge pull request #29 from ami-iit/add-prismatic-joints
Browse files Browse the repository at this point in the history 
Add prismatic joints
  • Loading branch information
@Giulero
Giulero authored Oct 5, 2022
2 parents c5bdd34 + c9dc1c1 commit 5d50695
Show file tree
Hide file tree
Showing 2 changed files with 121 additions and 38 deletions.
112 changes: 74 additions & 38 deletions src/adam/core/rbd_algorithms.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,23 @@ def crba(
joint_i.axis[1],
joint_i.axis[2],
)
elif joint_i.type in ["prismatic"]:
q = joint_positions[joint_i.idx] if joint_i.idx is not None else 0.0
X_J = self.X_prismatic_joint(
joint_i.origin.xyz,
joint_i.origin.rpy,
joint_i.axis,
q,
)
X_p[i] = X_J
Phi[i] = self.vertcat(
joint_i.axis[0],
joint_i.axis[1],
joint_i.axis[2],
0,
0,
0,
)

for i in range(self.tree.N - 1, -1, -1):
link_i = self.tree.links[i]
Expand Down Expand Up @@ -193,9 +210,19 @@ def forward_kinematics(
q_,
)
T_fk = T_fk @ T_joint
elif joint.type in ["prismatic"]:
# if the joint is actuated set the value
q_ = joint_positions[joint.idx] if joint.idx is not None else 0.0
T_joint = self.H_prismatic_joint(
joint.origin.xyz,
joint.origin.rpy,
joint.axis,
q_,
)
T_fk = T_fk @ T_joint
return T_fk

def jacobian(
def joints_jacobian(
self, frame: str, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike
) -> npt.ArrayLike:
"""Returns the Jacobian relative to the specified frame
Expand All @@ -206,11 +233,10 @@ def jacobian(
joint_positions (npt.ArrayLike): The joints position
Returns:
J_tot (npt.ArrayLike): The Jacobian relative to the frame
J (npt.ArrayLike): The Joints Jacobian relative to the frame
"""
chain = self.robot_desc.get_chain(self.root_link, frame, links=False)
T_fk = self.eye(4)
T_fk = T_fk @ base_transform
T_fk = self.eye(4) @ base_transform
J = self.zeros(6, self.NDoF)
T_ee = self.forward_kinematics(frame, base_transform, joint_positions)
P_ee = T_ee[:3, 3]
Expand All @@ -232,15 +258,37 @@ def jacobian(
T_fk = T_fk @ T_joint
p_prev = P_ee - T_fk[:3, 3].array
z_prev = T_fk[:3, :3] @ joint.axis
# J[:, joint.idx] = self.vertcat(
# cs.jacobian(P_ee, joint_positions[joint.idx]), z_prev) # using casadi jacobian
if joint.idx is not None:
J[:, joint.idx] = self.vertcat(self.skew(z_prev) @ p_prev, z_prev)
J_lin = self.skew(z_prev) @ p_prev
J_ang = z_prev

if joint.type in ["prismatic"]:
q_ = joint_positions[joint.idx] if joint.idx is not None else 0.0
T_joint = self.H_prismatic_joint(
joint.origin.xyz,
joint.origin.rpy,
joint.axis,
q_,
)
T_fk = T_fk @ T_joint
z_prev = T_fk[:3, :3] @ joint.axis
J_lin = z_prev
J_ang = self.zeros(3)

if joint.idx is not None:
J[:, joint.idx] = self.vertcat(J_lin, J_ang)

return J

def jacobian(
self, frame: str, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike
) -> npt.ArrayLike:

J = self.joints_jacobian(frame, base_transform, joint_positions)
T_ee = self.forward_kinematics(frame, base_transform, joint_positions)
# Adding the floating base part of the Jacobian, in Mixed representation
J_tot = self.zeros(6, self.NDoF + 6)
J_tot[:3, :3] = self.eye(3)
J_tot[:3, 3:6] = -self.skew((P_ee - base_transform[:3, 3]))
J_tot[:3, 3:6] = -self.skew((T_ee[:3, 3] - base_transform[:3, 3]))
J_tot[:3, 6:] = J[:3, :]
J_tot[3:, 3:6] = self.eye(3)
J_tot[3:, 6:] = J[3:, :]
Expand All @@ -258,36 +306,8 @@ def relative_jacobian(
Returns:
J (npt.ArrayLike): The Jacobian between the root and the frame
"""
chain = self.robot_desc.get_chain(self.root_link, frame, links=False)
base_transform = self.eye(4).array
T_fk = self.eye(4)
T_fk = T_fk @ base_transform
J = self.zeros(6, self.NDoF)
T_ee = self.forward_kinematics(frame, base_transform, joint_positions)
P_ee = T_ee[:3, 3]
for item in chain:
joint = self.robot_desc.joint_map[item]
if joint.type == "fixed":
xyz = joint.origin.xyz
rpy = joint.origin.rpy
joint_frame = self.H_from_Pos_RPY(xyz, rpy)
T_fk = T_fk @ joint_frame
if joint.type in ["revolute", "continuous"]:
q = joint_positions[joint.idx] if joint.idx is not None else 0.0
T_joint = self.H_revolute_joint(
joint.origin.xyz,
joint.origin.rpy,
joint.axis,
q,
)
T_fk = T_fk @ T_joint
p_prev = P_ee - T_fk[:3, 3]
z_prev = T_fk[:3, :3] @ joint.axis
# J[:, joint.idx] = self.vertcat(
# cs.jacobian(P_ee, joint_positions[joint.idx]), z_prev) # using casadi jacobian
if joint.idx is not None:
J[:, joint.idx] = self.vertcat(self.skew(z_prev) @ p_prev, z_prev)
return J
return self.joints_jacobian(frame, base_transform, joint_positions)

def CoM_position(
self, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike
Expand Down Expand Up @@ -408,6 +428,22 @@ def rnea(
0, 0, 0, joint_i.axis[0], joint_i.axis[1], joint_i.axis[2]
)
v_J = Phi[i] * q_dot
elif joint_i.type in ["prismatic"]:
q = joint_positions[joint_i.idx] if joint_i.idx is not None else 0.0
q_dot = (
joint_velocities[joint_i.idx] if joint_i.idx is not None else 0.0
)
X_J = self.X_prismatic_joint(
joint_i.origin.xyz,
joint_i.origin.rpy,
joint_i.axis,
q,
)
X_p[i] = X_J
Phi[i] = self.vertcat(
joint_i.axis[0], joint_i.axis[1], joint_i.axis[2], 0, 0, 0
)
v_J = Phi[i] * q_dot

if link_i.name == self.root_link:
v[i] = v_J
Expand Down
47 changes: 47 additions & 0 deletions src/adam/core/spatial_math.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,6 +163,30 @@ def H_revolute_joint(
T[:3, 3] = xyz
return T

@classmethod
def H_prismatic_joint(
cls,
xyz: npt.ArrayLike,
rpy: npt.ArrayLike,
axis: npt.ArrayLike,
q: npt.ArrayLike,
) -> npt.ArrayLike:
"""
Args:
xyz (npt.ArrayLike): joint origin in the urdf
rpy (npt.ArrayLike): joint orientation in the urdf
axis (npt.ArrayLike): joint axis in the urdf
q (npt.ArrayLike): joint angle value
Returns:
npt.ArrayLike: Homogeneous transform
"""
T = cls.eye(4)
R = cls.R_from_RPY(rpy)
T[:3, :3] = R
T[:3, 3] = xyz + q * cls.array(axis)
return T

@classmethod
def H_from_Pos_RPY(cls, xyz: npt.ArrayLike, rpy: npt.ArrayLike) -> npt.ArrayLike:
"""
Expand Down Expand Up @@ -213,6 +237,29 @@ def X_revolute_joint(
p = -T[:3, :3].T @ T[:3, 3]
return cls.spatial_transform(R, p)

@classmethod
def X_prismatic_joint(
cls,
xyz: npt.ArrayLike,
rpy: npt.ArrayLike,
axis: npt.ArrayLike,
q: npt.ArrayLike,
) -> npt.ArrayLike:
"""
Args:
xyz (npt.ArrayLike): joint origin in the urdf
rpy (npt.ArrayLike): joint orientation in the urdf
axis (npt.ArrayLike): joint axis in the urdf
q (npt.ArrayLike): joint angle value
Returns:
npt.ArrayLike: Spatial transform of a prismatic joint given its increment
"""
T = cls.H_prismatic_joint(xyz, rpy, axis, q)
R = T[:3, :3].T
p = -T[:3, :3].T @ T[:3, 3]
return cls.spatial_transform(R, p)

@classmethod
def X_fixed_joint(cls, xyz: npt.ArrayLike, rpy: npt.ArrayLike) -> npt.ArrayLike:
"""
Expand Down

0 comments on commit 5d50695

Please sign in to comment.