Skip to content

Commit

Permalink
Merge pull request #24 from ami-iit/fix_get_chain
Browse files Browse the repository at this point in the history 
Fix get_chain call for forward kinematics and jacobian functions.  
Consider only links with inertia in computations.
  • Loading branch information
@Giulero
Giulero authored Jun 17, 2022
2 parents f3e25c5 + 9652e84 commit 94b3bc1
Show file tree
Hide file tree
Showing 2 changed files with 73 additions and 94 deletions.
153 changes: 65 additions & 88 deletions src/adam/core/rbd_algorithms.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,16 +8,6 @@
import dataclasses


@dataclasses.dataclass
class CustomInertia:
ixx: float
ixy: float
ixz: float
iyy: float
iyz: float
izz: float


class RBDAlgorithms(SpatialMath):
"""This is a small abstract class that implements Rigid body algorithms retrieving robot quantities represented
in mixed representation, for Floating Base systems - as humanoid robots.
Expand Down Expand Up @@ -165,16 +155,10 @@ def crba(
return M, Jcm

def extract_link_properties(self, link_i):
if link_i.inertial is not None:
I = link_i.inertial.inertia
mass = link_i.inertial.mass
o = link_i.inertial.origin.xyz
rpy = link_i.inertial.origin.rpy
else:
I = CustomInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
mass = 0.0
o = [0.0, 0.0, 0.0]
rpy = [0.0, 0.0, 0.0]
I = link_i.inertial.inertia
mass = link_i.inertial.mass
o = link_i.inertial.origin.xyz
rpy = link_i.inertial.origin.rpy
return I, mass, o, rpy

def forward_kinematics(
Expand All @@ -189,27 +173,26 @@ def forward_kinematics(
Returns:
T_fk (npt.ArrayLike): The fk represented as Homogenous transformation matrix
"""
chain = self.robot_desc.get_chain(self.root_link, frame)
chain = self.robot_desc.get_chain(self.root_link, frame, links=False)
T_fk = self.eye(4)
T_fk = T_fk @ base_transform
for item in chain:
if item in self.robot_desc.joint_map:
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"]:
# 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_revolute_joint(
joint.origin.xyz,
joint.origin.rpy,
joint.axis,
q_,
)
T_fk = T_fk @ T_joint
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"]:
# 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_revolute_joint(
joint.origin.xyz,
joint.origin.rpy,
joint.axis,
q_,
)
T_fk = T_fk @ T_joint
return T_fk

def jacobian(
Expand All @@ -225,37 +208,34 @@ def jacobian(
Returns:
J_tot (npt.ArrayLike): The Jacobian relative to the frame
"""
chain = self.robot_desc.get_chain(self.root_link, frame)
chain = self.robot_desc.get_chain(self.root_link, frame, links=False)
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:
if item in self.robot_desc.joint_map:
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].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
)
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].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)

# Adding the floating base part of the Jacobian, in Mixed representation
J_tot = self.zeros(6, self.NDoF + 6)
Expand All @@ -278,38 +258,35 @@ 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)
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:
if item in self.robot_desc.joint_map:
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
)
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

def CoM_position(
Expand Down
14 changes: 8 additions & 6 deletions src/adam/core/urdf_tree.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
# This software may be modified and distributed under the terms of the
# GNU Lesser General Public License v2.1 or any later version.

import contextlib
import logging
from dataclasses import dataclass, field
from os import error
Expand Down Expand Up @@ -86,16 +87,14 @@ def load_model(self):
table_frames = PrettyTable(["Idx", "Frame name", "Parent"])
table_frames.title = "Frames"
for [i, item] in enumerate(frames):
try:
with contextlib.suppress(Exception):
table_frames.add_row(
[
i,
item,
self.robot_desc.parent_map[item][1],
]
)
except Exception:
pass
logging.debug(table_frames)
"""The node 0 contains the 1st link, the fictitious joint that connects the root the the world
and the world"""
Expand All @@ -117,9 +116,12 @@ def load_model(self):
parent = self.robot_desc.joint_map[item].parent
child = self.robot_desc.joint_map[item].child
if (
self.robot_desc.link_map[self.robot_desc.joint_map[item].parent]
in tree.links
and self.robot_desc.joint_map[item] not in tree.joints
self.robot_desc.link_map[parent]
in tree.links # this line preserves the order in the tree
and self.robot_desc.joint_map[item]
not in tree.joints # if the joint is present in the list of joints, no element is added
and self.robot_desc.link_map[child].inertial
is not None # if the child is a frame (massless link), no element is added
):
joints += [item]
table_joints.add_row(
Expand Down

0 comments on commit 94b3bc1

Please sign in to comment.