def XJT_revolute_BA(xyz, rpy, axis, qi):
"""Returns the spatial transform from parent link to child link with
a revolute connecting joint."""
T = tm.revolute(xyz, rpy, axis, qi)
rotation_matrix = T[:3, :3]
displacement = T[:3, 3]
return spatial_transform_BA(rotation_matrix, displacement)
def get_forward_kinematics(self, root, tip):
"""Returns the forward kinematics as a casadi function."""
chain = self.robot_desc.get_chain(root, tip)
if self.robot_desc is None:
raise ValueError('Robot description not loaded from urdf')
joint_list, actuated_names, upper, lower = self.get_joint_info(
root, tip)
nvar = len(actuated_names)
T_fk = cs.SX.eye(4)
q = cs.SX.sym("q", nvar)
quaternion_fk = cs.SX.zeros(4)
quaternion_fk[3] = 1.0
dual_quaternion_fk = cs.SX.zeros(8)
dual_quaternion_fk[3] = 1.0
i = 0
for joint in joint_list:
if joint.type == "fixed":
xyz = joint.origin.xyz
rpy = joint.origin.rpy
joint_frame = T.numpy_rpy(xyz, *rpy)
joint_quaternion = quaternion.numpy_rpy(*rpy)
joint_dual_quat = dual_quaternion.numpy_prismatic(
xyz, rpy, [1., 0., 0.], 0.)
T_fk = cs.mtimes(T_fk, joint_frame)
quaternion_fk = quaternion.product(quaternion_fk,
joint_quaternion)
dual_quaternion_fk = dual_quaternion.product(
dual_quaternion_fk, joint_dual_quat)
elif joint.type == "prismatic":
if joint.axis is None:
axis = cs.np.array([1., 0., 0.])
else:
axis = cs.np.array(joint.axis)
# axis = (1./cs.np.linalg.norm(axis))*axis
joint_frame = T.prismatic(joint.origin.xyz, joint.origin.rpy,
joint.axis, q[i])
joint_quaternion = quaternion.numpy_rpy(*joint.origin.rpy)
joint_dual_quat = dual_quaternion.prismatic(
joint.origin.xyz, joint.origin.rpy, axis, q[i])
T_fk = cs.mtimes(T_fk, joint_frame)
quaternion_fk = quaternion.product(quaternion_fk,
joint_quaternion)
dual_quaternion_fk = dual_quaternion.product(
dual_quaternion_fk, joint_dual_quat)
i += 1
elif joint.type in ["revolute", "continuous"]:
if joint.axis is None:
axis = cs.np.array([1., 0., 0.])
else:
axis = cs.np.array(joint.axis)
axis = (1. / cs.np.linalg.norm(axis)) * axis
joint_frame = T.revolute(joint.origin.xyz, joint.origin.rpy,
joint.axis, q[i])
joint_quaternion = quaternion.revolute(joint.origin.xyz,
joint.origin.rpy, axis,
q[i])
joint_dual_quat = dual_quaternion.revolute(
joint.origin.xyz, joint.origin.rpy, axis, q[i])
T_fk = cs.mtimes(T_fk, joint_frame)
quaternion_fk = quaternion.product(quaternion_fk,
joint_quaternion)
dual_quaternion_fk = dual_quaternion.product(
dual_quaternion_fk, joint_dual_quat)
i += 1
T_fk = cs.Function("T_fk", [q], [T_fk], self.func_opts)
quaternion_fk = cs.Function("quaternion_fk", [q], [quaternion_fk],
self.func_opts)
dual_quaternion_fk = cs.Function("dual_quaternion_fk", [q],
[dual_quaternion_fk], self.func_opts)
return {
"joint_names": actuated_names,
"upper": upper,
"lower": lower,
"joint_list": joint_list,
"q": q,
"quaternion_fk": quaternion_fk,
"dual_quaternion_fk": dual_quaternion_fk,
"T_fk": T_fk
}