def test_prismatic_joint():
urdf = """
<?xml version="1.0"?>
<robot name="mmm">
<link name="parent"/>
<link name="child"/>
<joint name="joint" type="prismatic">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="parent"/>
<child link="child"/>
<axis xyz="1 0 0"/>
</joint>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
tm.set_joint("joint", 5.33)
c2p = tm.get_transform("child", "parent")
assert_array_almost_equal(
c2p,
np.array([[1, 0, 0, 5.33],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
)
def test_joint_angles():
tm = UrdfTransformManager()
tm.load_urdf(COMPI_URDF)
for i in range(1, 7):
tm.set_joint("joint%d" % i, 0.1 * i)
link7_to_linkmount = tm.get_transform("link6", "linkmount")
assert_array_almost_equal(
link7_to_linkmount,
np.array([[0.121698, -0.606672, 0.785582, 0.489351],
[0.818364, 0.509198, 0.266455, 0.114021],
[-0.561668, 0.610465, 0.558446, 0.924019], [0., 0., 0.,
1.]]))
def test_plot_mesh_smoke_with_scale():
if not matplotlib_available:
raise SkipTest("matplotlib is required for this test")
BASE_DIR = "test/test_data/"
tm = UrdfTransformManager()
with open(BASE_DIR + "simple_mechanism.urdf", "r") as f:
tm.load_urdf(f.read(), mesh_path=BASE_DIR)
tm.set_joint("joint", -1.1)
ax = tm.plot_frames_in(
"lower_cone", s=0.1, whitelist=["upper_cone", "lower_cone"], show_name=True)
ax = tm.plot_connections_in("lower_cone", ax=ax)
tm.plot_visuals("lower_cone", ax=ax)
def test_plot_without_mesh():
if not matplotlib_available:
raise SkipTest("matplotlib is required for this test")
BASE_DIR = "test/test_data/"
tm = UrdfTransformManager()
with open(BASE_DIR + "simple_mechanism.urdf", "r") as f:
tm.load_urdf(f.read(), mesh_path=None)
tm.set_joint("joint", -1.1)
ax = tm.plot_frames_in(
"lower_cone", s=0.1, whitelist=["upper_cone", "lower_cone"], show_name=True)
ax = tm.plot_connections_in("lower_cone", ax=ax)
with warnings.catch_warnings(record=True) as w:
tm.plot_visuals("lower_cone", ax=ax)
assert_equal(len(w), 1)
def test_joint_limit_clipping():
tm = UrdfTransformManager()
tm.load_urdf(COMPI_URDF)
tm.set_joint("joint1", 2.0)
link7_to_linkmount = tm.get_transform("link6", "linkmount")
assert_array_almost_equal(
link7_to_linkmount,
np.array([[0.5403023, -0.8414710, 0, 0], [0.8414710, 0.5403023, 0, 0],
[0, 0, 1, 1.124], [0, 0, 0, 1]]))
tm.set_joint("joint1", -2.0)
link7_to_linkmount = tm.get_transform("link6", "linkmount")
assert_array_almost_equal(
link7_to_linkmount,
np.array([[0.5403023, 0.8414710, 0, 0], [-0.8414710, 0.5403023, 0, 0],
[0, 0, 1, 1.124], [0, 0, 0, 1]]))
def test_plot_mesh_smoke_without_scale():
if not matplotlib_available:
raise SkipTest("matplotlib is required for this test")
urdf = """
<?xml version="1.0"?>
<robot name="simple_mechanism">
<link name="upper_cone">
<visual name="upper_cone">
<origin xyz="0 0 0" rpy="0 1.5708 0"/>
<geometry>
<mesh filename="cone.stl"/>
</geometry>
</visual>
</link>
<link name="lower_cone">
<visual name="lower_cone">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="cone.stl"/>
</geometry>
</visual>
</link>
<joint name="joint" type="revolute">
<origin xyz="0 0 0.2" rpy="0 0 0"/>
<parent link="lower_cone"/>
<child link="upper_cone"/>
<axis xyz="1 0 0"/>
<limit lower="-2.79253" upper="2.79253" effort="0" velocity="0"/>
</joint>
</robot>
"""
BASE_DIR = "test/test_data/"
tm = UrdfTransformManager()
tm.load_urdf(urdf, mesh_path=BASE_DIR)
tm.set_joint("joint", -1.1)
ax = tm.plot_frames_in("lower_cone",
s=0.1,
whitelist=["upper_cone", "lower_cone"],
show_name=True)
ax = tm.plot_connections_in("lower_cone", ax=ax)
tm.plot_visuals("lower_cone", ax=ax)
def test_with_empty_axis():
urdf = """
<robot name="robot_name">
<link name="link0"/>
<link name="link1"/>
<joint name="joint0" type="revolute">
<parent link="link0"/>
<child link="link1"/>
<origin/>
<axis/>
</joint>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
tm.set_joint("joint0", np.pi)
link1_to_link0 = tm.get_transform("link1", "link0")
assert_array_almost_equal(
link1_to_link0,
np.array([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]))
class RemoteHelloRobot(object):
"""Hello Robot interface"""
def __init__(self):
self._robot = Robot()
self._robot.startup()
if not self._robot.is_calibrated():
self._robot.home()
self._robot.stow()
self._done = True
# Read battery maintenance guide https://docs.hello-robot.com/battery_maintenance_guide/
self._check_battery()
self._load_urdf()
def _check_battery(self):
p = self._robot.pimu
p.pull_status()
val_in_range("Voltage",
p.status["voltage"],
vmin=p.config["low_voltage_alert"],
vmax=14.0)
val_in_range("Current",
p.status["current"],
vmin=0.1,
vmax=p.config["high_current_alert"])
val_in_range("CPU Temp", p.status["cpu_temp"], vmin=15, vmax=80)
print(Style.RESET_ALL)
def _load_urdf(self):
import os
urdf_path = os.path.join(
os.getenv("HELLO_FLEET_PATH"),
os.getenv("HELLO_FLEET_ID"),
"exported_urdf",
"stretch.urdf",
)
from pytransform3d.urdf import UrdfTransformManager
import pytransform3d.transformations as pt
import pytransform3d.visualizer as pv
import numpy as np
self.tm = UrdfTransformManager()
with open(urdf_path, "r") as f:
urdf = f.read()
self.tm.load_urdf(urdf)
def get_camera_transform(self):
s = self._robot.get_status()
head_pan = s["head"]["head_pan"]["pos"]
head_tilt = s["head"]["head_tilt"]["pos"]
# Get Camera transform
self.tm.set_joint("joint_head_pan", head_pan)
self.tm.set_joint("joint_head_tilt", head_tilt)
camera_transform = self.tm.get_transform("camera_link", "base_link")
return camera_transform
def get_status(self):
return self._robot.get_status()
def get_base_state(self):
s = self._robot.get_status()
return (s["base"]["x"], s["base"]["y"], s["base"]["theta"])
def get_pan(self):
s = self._robot.get_status()
return s["head"]["head_pan"]["pos"]
def get_tilt(self):
s = self._robot.get_status()
return s["head"]["head_tilt"]["pos"]
def set_pan(self, pan):
self._robot.head.move_to("head_pan", pan)
def set_tilt(self, tilt):
self._robot.head.move_to("head_tilt", tilt)
def reset_camera(self):
self.set_pan(0)
self.set_tilt(0)
def set_pan_tilt(self, pan, tilt):
"""Sets both the pan and tilt joint angles of the robot camera to the
specified values.
:param pan: value to be set for pan joint in radian
:param tilt: value to be set for the tilt joint in radian
:type pan: float
:type tilt: float
:type wait: bool
"""
self._robot.head.move_to("head_pan", pan)
self._robot.head.move_to("head_tilt", tilt)
def test_connection(self):
print("Connected!!") # should print on server terminal
return "Connected!" # should print on client terminal
def home(self):
self._robot.home()
def stow(self):
self._robot.stow()
def push_command(self):
self._robot.push_command()
def translate_by(self, x_m):
self._robot.base.translate_by(x_m)
self._robot.push_command()
def rotate_by(self, x_r):
self._robot.base.rotate_by(x_r)
self._robot.push_command()
def go_to_absolute(self, xyt_position):
"""Moves the robot base to given goal state in the world frame.
:param xyt_position: The goal state of the form (x,y,yaw)
in the world (map) frame.
"""
if self._done:
self._done = False
global_xyt = xyt_position
base_state = self.get_base_state()
base_xyt = transform_global_to_base(global_xyt, base_state)
goto(self._robot, list(base_xyt), dryrun=False)
self._done = True
return self._done
def go_to_relative(self, xyt_position):
"""Moves the robot base to the given goal state relative to its current
pose.
:param xyt_position: The relative goal state of the form (x,y,yaw)
"""
if self._done:
self._done = False
goto(self._robot, list(xyt_position), dryrun=False)
self._done = True
def is_moving(self):
return not self._done
def stop(self):
robot.stop()
robot.push_command()
def remove_runstop(self):
if robot.pimu.status["runstop_event"]:
robot.pimu.runstop_event_reset()
robot.push_command()
<origin xyz="-0.00065 0.03005 -0.44359" rpy="0 0 0"/>
<parent link="link_RightLeg"/>
<child link="link_RightFoot"/>
<limit effort="0" velocity="0"/>
</joint>
<joint name="link_RightToeBase" type="revolute">
<origin xyz="0.00016 -0.08667 -0.08682" rpy="0 0 0"/>
<parent link="link_RightFoot"/>
<child link="link_RightToeBase"/>
<limit effort="0" velocity="0"/>
</joint>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(COMPI_URDF)
joint_names = [
"link_LeftUpLeg", "link_LeftLeg", "link_LeftFoot", "link_LeftToeBase",
"link_RightUpLeg", "link_RightLeg", "link_RightFoot", "link_RightToeBase"
]
joint_angles = [0, 0, 0, 0, 0, 0, 0]
for name, angle in zip(joint_names, joint_angles):
tm.set_joint(name, angle)
ax = tm.plot_frames_in("compi", whitelist=joint_names, s=0.05, show_name=True)
ax = tm.plot_connections_in("compi", ax=ax)
ax.set_xlim((-0.2, 0.8))
ax.set_ylim((-0.5, 0.5))
ax.set_zlim((-0.2, 0.8))
plt.show()
BASE_DIR = "test/test_data/"
data_dir = BASE_DIR
search_path = "."
while (not os.path.exists(data_dir)
and os.path.dirname(search_path) != "pytransform3d"):
search_path = os.path.join(search_path, "..")
data_dir = os.path.join(search_path, BASE_DIR)
tm = UrdfTransformManager()
filename = os.path.join(data_dir, "robot_with_visuals.urdf")
with open(filename, "r") as f:
robot_urdf = f.read()
tm.load_urdf(robot_urdf, mesh_path=data_dir)
tm.set_joint("joint2", 0.2 * np.pi)
tm.set_joint("joint3", 0.2 * np.pi)
tm.set_joint("joint5", 0.1 * np.pi)
tm.set_joint("joint6", 0.5 * np.pi)
ee2base = tm.get_transform("tcp", "robot_arm")
base2ee = tm.get_transform("robot_arm", "tcp")
mass = 1.0
wrench_in_ee = np.array([0.0, 0.0, 0.0, 0.0, -9.81, 0.0]) * mass
wrench_in_base = np.dot(pt.adjoint_from_transform(base2ee).T, wrench_in_ee)
fig = pv.figure()
fig.plot_graph(tm, "robot_arm", s=0.1, show_visuals=True)
BASE_DIR = "test/test_data/"
data_dir = BASE_DIR
search_path = "."
while (not os.path.exists(data_dir) and
os.path.dirname(search_path) != "pytransform3d"):
search_path = os.path.join(search_path, "..")
data_dir = os.path.join(search_path, BASE_DIR)
tm = UrdfTransformManager()
filename = os.path.join(data_dir, "robot_with_visuals.urdf")
with open(filename, "r") as f:
robot_urdf = f.read()
tm.load_urdf(robot_urdf, mesh_path=data_dir)
joint_names = ["joint%d" % i for i in range(1, 7)]
for joint_name in joint_names:
tm.set_joint(joint_name, 0.5)
fig = pv.figure()
graph = fig.plot_graph(
tm, "robot_arm", s=0.1, show_frames=True, show_visuals=True)
fig.view_init()
fig.set_zoom(1.5)
n_frames = 100
if "__file__" in globals():
fig.animate(animation_callback, n_frames, loop=True,
fargs=(n_frames, tm, graph, joint_names))
fig.show()
else:
fig.save_image("__open3d_rendered_image.jpg")
from pytransform3d.urdf import UrdfTransformManager
import pytransform3d.visualizer as pv
BASE_DIR = "test/test_data/"
data_dir = BASE_DIR
search_path = "."
while (not os.path.exists(data_dir) and
os.path.dirname(search_path) != "pytransform3d"):
search_path = os.path.join(search_path, "..")
data_dir = os.path.join(search_path, BASE_DIR)
tm = UrdfTransformManager()
with open(data_dir + "simple_mechanism.urdf", "r") as f:
tm.load_urdf(f.read(), mesh_path=data_dir)
tm.set_joint("joint", -1.1)
fig = pv.figure()
# add graph, box, and sphere
graph = fig.plot_graph(
tm, "lower_cone", s=0.1, show_frames=True,
whitelist=["upper_cone", "lower_cone"],
show_connections=True, show_visuals=True, show_name=False)
box = fig.plot_box([1, 1, 1])
sphere = fig.plot_sphere(2)
# remove graph and box
fig.remove_artist(graph)
fig.remove_artist(box)
class Atlas():
""" This class represents a 15 DOF (revolute joints only) serial chain of the humanoid Atlas.
The investigated chain is the following:
['l_foot', 'l_talus', 'l_lleg', 'l_uleg', 'l_lglut', 'l_uglut', 'pelvis', 'ltorso', 'mtorso',\
'utorso', 'l_clav', 'l_scap', 'l_uarm', 'l_larm', 'l_farm', 'l_hand', 'tcp']
"""
def __init__(self, urdf_path, theta_stds=None):
""" Constructor:
Params:
urdf_path: string: path to the URDF file containing the model of the manipulator
including all relevant parameters
theta_stds: 15x1-vector: standard deviations of the joint accuracies of the manipulator (rad)
"""
with open(urdf_path) as f:
urdf_string = f.read()
self.tm = UrdfTransformManager()
self.tm.load_urdf(urdf_string)
self.n_DOFS = 15
self.has_jacobian = False
self.has_IK = False
self.has_singularity_condition = False
#link names of the investigated subchain of Atlas from base to end effector (15-DOF)
self.sorted_link_names = ['l_foot', 'l_talus', 'l_lleg', 'l_uleg', 'l_lglut', 'l_uglut', 'pelvis', 'ltorso', 'mtorso',\
'utorso', 'l_clav', 'l_scap', 'l_uarm', 'l_larm', 'l_farm', 'l_hand', 'tcp']
self.joint_info = self._extract_relevant_joints()
#joint names of the investigated subchain of Atlas from base to end effector (15-DOF)
self.sorted_joint_names = self._get_sorted_joint_names()
if theta_stds is None:
#default is an uncertainty of a standard deviation of 1 degree per joint
self.theta_stds = np.zeros(self.n_DOFS) + (1 * math.pi / 180)
else:
self.theta_stds = theta_stds
self.joint_transformations_notheta = self._construct_transformations_notheta(
)
self.summands2, self.summands3 = self._prepare_rodrigues_summands()
#trajectory that is tested in one of the experiments
#numbers are: [start_x, end_x, start_y, end_y, start_z, end_z]
self.trajectory = [-0.5, -0.1, -0.3, 0.3, 1.3, 1.8]
#arbitrarily chosen subspace of the maximum workspace (for no joint limits) where we train and test
#it is secured via numerical tests that this set really lies in the workspace
self.subspace = np.array([[0.7, 1.2], [-0.4, 0.1], [1,
1.5], [0.6, 0.6],
[0.1, 0.1], [0.1, 0.1], [0, 0.5]])
def _extract_relevant_joints(self):
""" Removes the joints not belonging to the investigated chain of Atlas.
"""
joint_info = self.tm._joints.copy()
for joint_name in list(self.tm._joints):
if self.tm._joints[joint_name][
0] not in self.sorted_link_names or self.tm._joints[
joint_name][1] not in self.sorted_link_names:
joint_info.pop(joint_name)
return joint_info
def _get_sorted_joint_names(self):
""" Collects the joint names in a sorted list where the order corresponds to the the chain from the
robot base to the end effector.
"""
sorted_joints = []
for i in range(len(self.sorted_link_names) - 1):
for joint in self.joint_info:
if self.sorted_link_names[i] in self.joint_info[
joint][:2] and self.sorted_link_names[
i + 1] in self.joint_info[joint][:2]:
sorted_joints.append(joint)
return sorted_joints
def _construct_transformations_notheta(self):
""" Prepares the transformation matrices for this manipulator between two adjacent joints.
They are valid only for theta_i = 0 for all i, i.e. the default configuration. Later, these
matrices are re-used in the FK calculation.
Returns:
transformations_notheta: 7x4x4-matrix: contains the transformation matrices for adjacent joints
in the case of theta_i = 0 for all i
"""
transformations_notheta = np.zeros((self.n_DOFS + 1, 4, 4))
for i, joint in enumerate(self.sorted_joint_names):
#take the child to parent transformation matrices for the default configuration
transformations_notheta[i] = self.joint_info[joint][2]
#static tcp transformation
transformations_notheta[-1] = self.tm.get_transform(
self.sorted_link_names[-1], self.sorted_link_names[-2])
return transformations_notheta
def _prepare_rodrigues_summands(self):
""" Prepares the second summand of the decomposed Rodrigues formula which is used in the FK calculation.
This matrix does not depend on theta, therefore it is pre-calculated to speed up the FK calculation at runtime.
Returns: the nx4x4 matrices for the second and third summand of the decomposed Rodrigues formula
"""
#source of the decomposed Rodrigues formula: https://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/
summand2 = np.zeros((self.n_DOFS, 4, 4))
summand3 = np.zeros((self.n_DOFS, 4, 4))
for i, joint in enumerate(self.sorted_joint_names):
#get rotation axis of the joint
x, y, z = self.joint_info[joint][3]
summand2[i, :3, :3] = np.array([[x * x, x * y, x * z],
[x * y, y * y, y * z],
[x * z, y * z, z * z]])
summand3[i, :3, :3] = np.array([[0, -z, y], [z, 0, -x], [-y, x,
0]])
return summand2, summand3
def _test_jacobian(self, J, theta_vec):
""" Checks a jacobian for correctness. This function compares delta_x1 = J(theta) * delta_theta
and delta_x2 = FK(theta) - FK(theta + delta_theta) for a very small vector delta_theta.
If delta_x1 and delta_x2 are not close, an exception is thrown.
Params:
J: 6x15-matrix: The manipulator jacobian at theta_vec
theta_vec: 1x15-vector: The joint angles for which the Jacobian is valid
"""
theta_vec = np.atleast_2d(theta_vec)
delta_theta_small = np.zeros((self.n_DOFS, 1))
std = 1e-4
delta_theta_small[:, 0] = np.random.normal(loc=0,
scale=std,
size=self.n_DOFS)
old_jc = theta_vec
new_jc = theta_vec + delta_theta_small.T
true_T = self.FK(old_jc)[0]
new_T = self.FK(new_jc)[0]
true_quat = quaternion_from_matrix(true_T[:3, :3])
new_quat = quaternion_from_matrix(new_T[:3, :3])
pos_error = np.sqrt(np.sum(np.square(new_T[:3, 3] - true_T[:3, 3])))
ori_error = 2 * np.arccos(np.abs(np.inner(new_quat, true_quat)))
cart_error_j = np.dot(J, delta_theta_small)
pos_error_j = np.sqrt(np.sum(np.square(cart_error_j[:3])))
ori_error_j = np.sqrt(np.sum(np.square(cart_error_j[3:])))
threshold = 1e-7
assert np.abs(pos_error -
pos_error_j) < threshold, "Jacobian test failed"
assert np.abs(ori_error -
ori_error_j) < threshold, "Jacobian test failed"
def FK(self, thetas):
""" Wrapper for the Forward Kinematic Model which transforms joint configurations
into cartesian space.
Params:
thetas: nx15-matrix or -tensor: angles of the joints (rad)
rows: samples
columns: joints
Returns:
Ts: nx4x4-matrix: homogenous transformation matrices
n: number of samples
X[i,:,:]: homogenous transformation matrix of the ith sample
representing the end effector pose
"""
sess = tf.Session()
with sess.as_default():
thetas_T = tf.convert_to_tensor(thetas)
Ts = self.FK_tensor(thetas_T).eval()
return Ts
def FK_tensor(self, thetas):
""" Forward Kinematic Model:
Transforms joint configurations into 3D cartesian space.
Params:
thetas: nx15-tensor: angles of the joints (rad)
rows: samples
columns: joints
Returns:
_0_T_tcp: nx4x4-tensor: homogenous transformation matrices
n: number of samples
X[i,:,:]: homogenous transformation matrix of the ith sample
representing the end effector pose
"""
#make sure the datatypes are all float64
thetas = tf.cast(thetas, dtype=tf.float64)
#convert preliminary transformation matrices to tensors
m1 = [tf.shape(thetas)[0], 1, 1, 1]
m2 = [tf.shape(thetas)[0], 1, 1]
trans_notheta = tf.tile(
tf.expand_dims(
tf.convert_to_tensor(self.joint_transformations_notheta[:15],
dtype=tf.float64), 0), m1) #nx16x4x4
trans_tcp = tf.tile(
tf.expand_dims(
tf.convert_to_tensor(self.joint_transformations_notheta[15],
dtype=tf.float64), 0), m2) #nx4x4
#use the decomposed Rodrigues formula (source: https://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/)
identity = tf.tile(
tf.expand_dims(tf.eye(4, batch_shape=[15], dtype=tf.float64), 0),
m1) #nx15x4x4
summands2 = tf.expand_dims(
tf.convert_to_tensor(self.summands2, dtype=tf.float64),
0) #1x15x4x4
summands3 = tf.expand_dims(
tf.convert_to_tensor(self.summands3, dtype=tf.float64),
0) #1x15x4x4
Sth = tf.reshape(tf.sin(thetas), [-1, 15, 1, 1]) #nx15x1x1
Cth = tf.reshape(tf.cos(thetas), [-1, 15, 1, 1]) #nx15x1x1
tth = 1 - Cth #nx15x1x1
scaled_identity = tf.multiply(Cth, identity[:, :, :3, :3]) #nx15x3x3
tmp = tf.concat([scaled_identity, identity[:, :, :3, 3:]],
axis=-1) #nx15x3x4
scaled_homogenous_summands1 = tf.concat([tmp, identity[:, :, 3:, :]],
axis=2) #nx15x4x4
trans_theta = scaled_homogenous_summands1 + tf.multiply(
tth, summands2) + tf.multiply(Sth, summands3) #nx16x4x4
_i_T_ip1 = tf.matmul(trans_notheta, trans_theta) #nx15x4x4
#matrix multiplication of all transformation matrices
_2_T_0 = tf.matmul(_i_T_ip1[:, 1, :, :], _i_T_ip1[:, 0, :, :]) #nx4x4
_4_T_2 = tf.matmul(_i_T_ip1[:, 3, :, :], _i_T_ip1[:, 2, :, :]) #nx4x4
_6_T_4 = tf.matmul(_i_T_ip1[:, 5, :, :], _i_T_ip1[:, 4, :, :]) #nx4x4
_6_T_0 = tf.matmul(_6_T_4, tf.matmul(_4_T_2, _2_T_0))
#this matrix needs to be inversed because the investigated chain spans two branches of the structural tree of the URDF.
#the root node (index 6) is the pelvis
#from index 0 to 6 we go upwards from a leaf towards the root node
#from index 6 to 16 we go downwards from the root towards another leaf
#due to this, the order of parent and child changes which makes the inversion necessary at this point
#homogenous transformation matrices can be inverted faster by using a transposition:
R_t = tf.matrix_transpose(_6_T_0[:, :3, :3]) #nx3x3
t = _6_T_0[:, :3, 3:] #nx3x1
mRt = -tf.matmul(R_t, t) #nx3x1
row4 = identity[:, 0, 3:, :] #nx1x4
row123 = tf.concat([R_t, mRt], axis=-1) #nx3x4
_0_T_6 = tf.concat([row123, row4], axis=1) #nx4x4
_6_T_8 = tf.matmul(_i_T_ip1[:, 6, :, :], _i_T_ip1[:, 7, :, :]) #nx4x4
_8_T_10 = tf.matmul(_i_T_ip1[:, 8, :, :], _i_T_ip1[:, 9, :, :]) #nx4x4
_10_T_12 = tf.matmul(_i_T_ip1[:, 10, :, :], _i_T_ip1[:,
11, :, :]) #nx4x4
_12_T_14 = tf.matmul(_i_T_ip1[:, 12, :, :], _i_T_ip1[:,
13, :, :]) #nx4x4
_14_T_tcp = tf.matmul(_i_T_ip1[:, 14, :, :], trans_tcp) #nx4x4
_6_T_10 = tf.matmul(_6_T_8, _8_T_10)
_10_T_14 = tf.matmul(_10_T_12, _12_T_14)
_0_T_10 = tf.matmul(_0_T_6, _6_T_10)
_10_T_tcp = tf.matmul(_10_T_14, _14_T_tcp)
_0_T_tcp = tf.matmul(_0_T_10, _10_T_tcp)
return _0_T_tcp
def plot_configurations(self, joint_configurations):
""" Plots manipulator configurations in 3D cartesian space.
Params:
joint_configurations: nx15-matrix: angles of the joints (rad)
n: number of samples
Returns:
the figure and axis references
"""
for i in range(joint_configurations.shape[0]):
joint_names = self.sorted_joint_names
joint_angles = joint_configurations[i, :]
for name, angle in zip(joint_names, joint_angles):
self.tm.set_joint(name, angle)
if i == 0:
ax = self.tm.plot_frames_in("l_foot",
s=0.05,
show_name=False,
whitelist=self.sorted_link_names)
else:
ax = self.tm.plot_frames_in("l_foot",
s=0.05,
ax=ax,
show_name=False,
whitelist=self.sorted_link_names)
ax = self.tm.plot_connections_in("l_foot",
ax=ax,
whitelist=self.sorted_link_names)
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_zlabel('z [m]')
fig = plt.gcf()
return fig, ax
class Compi():
""" This class represents a 6 DOF RRR-RRR manipulator with a spherical wrist, e.g. Compi of the DFKI.
Link lengths and joint limits can be varied via the input URDF-file but the joint rotation axes and
the coordinate system relations (alpha) are fixed because analytical IK and Jacobian depend on it
and are hardcoded. This is due to the fact that the whole derivation process, especially of the analytical IK,
highly depends on those parameters and is difficult to generalize.
"""
def __init__(self, urdf_path, theta_stds=None):
""" Constructor:
Params:
urdf_path: path to the URDF file containing the model of the manipulator
including all relevant parameters
theta_stds: 6x1-vector: standard deviations of the joint accuracies of the manipulator (rad)
"""
with open(urdf_path) as f:
urdf_string = f.read()
self.tm = UrdfTransformManager()
self.tm.load_urdf(urdf_string)
self.n_DOFS = 6
self.has_jacobian = True
self.has_IK = True
self.has_singularity_condition = True
#sorted link names of the serial chain
self.sorted_link_names = ['base_link', 'link1', 'link2', 'link3', 'link4', 'link5', 'link6', 'tcp']
#sorted joint names of the serial chain
self.sorted_joint_names = self._get_sorted_joint_names()
self.joint_info = self.tm._joints.copy()
if theta_stds is None:
#default is an uncertainty of a standard deviation of 1 degree per joint
self.theta_stds = np.zeros(self.n_DOFS) + (1*math.pi/180)
else:
self.theta_stds = theta_stds
self.joint_transformations_notheta = self._construct_transformations_notheta()
#trajectory that is tested in one of the experiments
#numbers are: [start_x, end_x, start_y, end_y, start_z, end_z]
self.trajectory = [-0.5, -0.1, -0.3, 0.3, 0.4, 0.4]
#arbitrarily chosen subspace of the maximum workspace (for no joint limits) where we train and test
#it is secured via numerical tests that this set really lies in the workspace
self.subspace = np.array([[0, 0.5],
[0, 0.3],
[0.3, 0.6],
[0.8, 0.8],
[0.1, 0.1],
[0.1, 0.1],
[0, 0.3]])
def _get_sorted_joint_names(self):
""" Collects the joint names in a sorted list where the order corresponds to the the chain from the
robot base to the end effector.
"""
sorted_joints = []
for i in range(len(self.sorted_link_names)-1):
for joint in self.tm._joints:
if self.sorted_link_names[i] in self.tm._joints[joint][:2] and self.sorted_link_names[i+1] in self.tm._joints[joint][:2]:
sorted_joints.append(joint)
return sorted_joints
def _construct_transformations_notheta(self):
""" Prepares the transformation matrices for this manipulator between two adjacent joints.
The transformation matrices are pre-filled with all constant elements, i.e. link lengths,
rotation axes, angles alpha between two adjacent coordinate systems. The terms containing
the angle configuration theta are left out and need to be multiplied at a later stage.
Hence, this matrices are no valid homogenous transformation matrices in this form. However,
they enable a fast computation of the forward kinematics because the constant parts don't need
to be calculated over and over again.
Returns:
transformations_notheta: 7x4x4-matrix: contains the pre-filled matrices that can be turned
into homogenous transformation matrices easily
"""
#the construction of homogenous transformation matrices from robot parameters refers to
#"Modeling and Control of Manipulators, Part I: Geometric and Kinematic Models" by W. Khalil, page 40
#get link lengths
l1 = self.tm.get_transform('link2','link1')[2,3]
l2 = self.tm.get_transform('link3','link2')[0,3]
l3 = self.tm.get_transform('link4','link3')[1,3]
l4 = self.tm.get_transform('tcp','link6')[2,3]
transformations_notheta = np.zeros((7,4,4))
transformations_notheta[:,0,:2] = 1
transformations_notheta[:,3,3] = 1
#fill in the last columns containing the link lengths parameters
transformations_notheta[1,2,3] = l1
transformations_notheta[2,0,3] = l2
transformations_notheta[3,1,3] = l3
transformations_notheta[6,2,3] = l4
#fill in the alpha-factors in the second and third columns
alphas = [0, math.pi/2, 0, -math.pi/2, math.pi/2, -math.pi/2, 0]
for i, alpha in enumerate(alphas):
Ca = math.cos(alpha)
Sa = math.sin(alpha)
transformations_notheta[i,1,:3] = [Ca, Ca, -Sa]
transformations_notheta[i,2,:3] = [Sa, Sa, Ca]
#the last transformation (end effector) is a static one
transformations_notheta[6,:3,:3] = np.eye(3)
return transformations_notheta
def turn_configurations_singular(self, thetas):
#COMPI: elbow singularity: theta3=-pi/2 or theta3=pi/2
thetas[:,2] = -math.pi/2
return thetas
def jacobian(self, thetas, test=False):
""" Constructs jacobian matrices of the manipulator for given joint configurations.
Params:
thetas: nx6-matrix: angles of the joints (rad)
rows: samples
columns: joints
test: bool: whether to test the constructed jacobians for correctness
Returns:
J: nx6x6-matrix: jacobian matrices
n: number of samples
J[i,:,:]: jacobian matrix of the ith sample
"""
n = thetas.shape[0]
#the calculation of the jacobian for this 6-DOF RRR-RRR manipulator refers to
#"Modeling and Control of Manipulators, Part I: Geometric and Kinematic Models" by W. Khalil
#construct the kinematic jacobian (base to spherical wrist) first
#the last link from the spherical wrist to the end effector is not included
Jk = np.zeros((n,6,6))
l2 = self.tm.transforms[('link3','link2')][0,3]
l3 = self.tm.transforms[('link4','link3')][1,3]
C2 = np.cos(thetas[:,1])
C3 = np.cos(thetas[:,2])
C4 = np.cos(thetas[:,3])
C5 = np.cos(thetas[:,4])
C6 = np.cos(thetas[:,5])
S3 = np.sin(thetas[:,2])
S4 = np.sin(thetas[:,3])
S5 = np.sin(thetas[:,4])
S6 = np.sin(thetas[:,5])
C23 = np.cos(thetas[:,1] + thetas[:,2])
S23 = np.sin(thetas[:,1] + thetas[:,2])
Jk[:,0,0] = (- C6*C5*S4 - S6*C4)*(S23*l3 - C2*l2)
Jk[:,1,0] = (S6*C5*S4 - C6*C4)*(S23*l3 - C2*l2)
Jk[:,2,0] = S5*S4*(S23*l3 - C2*l2)
Jk[:,3,0] = (C6*C5*C4 - S6*S4)*S23 + C6*S5*C23
Jk[:,4,0] = (- S6*C5*C4 - C6*S4)*S23 - S6*S5*C23
Jk[:,5,0] = - S5*C4*S23 + C5*C23
Jk[:,0,1] = (- C6*C5*C4 + S6*S4)*(l3 - S3*l2) + C6*S5*C3*l2
Jk[:,1,1] = (S6*C5*C4 + C6*S4)*(l3 - S3*l2) - S6*S5*C3*l2
Jk[:,2,1] = S5*C4*(l3 - S3*l2) + C5*C3*l2
Jk[:,3,1] = - C6*C5*S4 - S6*C4
Jk[:,4,1] = S6*C5*S4 - C6*C4
Jk[:,5,1] = S5*S4
Jk[:,0,2] = (- C6*C5*C4 + S6*S4)*l3
Jk[:,1,2] = (S6*C5*C4 + C6*S4)*l3
Jk[:,2,2] = S5*C4*l3
Jk[:,3,2] = - C6*C5*S4 - S6*C4
Jk[:,4,2] = S6*C5*S4 - C6*C4
Jk[:,5,2] = S5*S4
Jk[:,0,3] = 0
Jk[:,1,3] = 0
Jk[:,2,3] = 0
Jk[:,3,3] = C6*S5
Jk[:,4,3] = - S6*S5
Jk[:,5,3] = C5
Jk[:,0,4] = 0
Jk[:,1,4] = 0
Jk[:,2,4] = 0
Jk[:,3,4] = - S6
Jk[:,4,4] = - C6
Jk[:,5,4] = 0
Jk[:,0,5] = 0
Jk[:,1,5] = 0
Jk[:,2,5] = 0
Jk[:,3,5] = 0
Jk[:,4,5] = 0
Jk[:,5,5] = 1
#include the relation of the last link by multiplying the kinematic jacobian
#with a screw transformation matrix
J = np.zeros((n,6,6))
for i in range(n):
screw_matrix = np.zeros((6,6))
#last transformation is fixed, therefore the following matrix is valid
#for all angle configurations
E_T_n = self.tm.get_transform(self.tm.nodes[-2], self.tm.nodes[-1])
E_R_n = E_T_n[:3,:3]
E_p_n = E_T_n[:3,3]
E_P_n_screw = np.array([[0, -E_p_n[2], E_p_n[1]],
[E_p_n[2], 0, -E_p_n[0]],
[-E_p_n[1], E_p_n[0], 0]])
screw_matrix[:3,:3] = E_R_n
screw_matrix[-3:,-3:] = E_R_n
screw_matrix[:3,-3:] = E_P_n_screw @ E_R_n
J[i] = screw_matrix @ Jk[i]
if test:
self._test_jacobian(J[i], thetas[i,:])
return J
def _test_jacobian(self, J, theta_vec):
""" Checks a jacobian for correctness. This function compares delta_x1 = J(theta) * delta_theta
and delta_x2 = FK(theta) - FK(theta + delta_theta) for a very small vector delta_theta.
If delta_x1 and delta_x2 are not close, an exception is thrown.
Params:
J: 6x6-matrix: The manipulator jacobian at theta_vec
theta_vec: 1x6-vector: The joint angles for which the Jacobian is valid
"""
theta_vec = np.atleast_2d(theta_vec)
delta_theta_small = np.zeros((self.n_DOFS,1))
std = 1e-4
delta_theta_small[:,0] = np.random.normal(loc=0, scale=std, size=self.n_DOFS)
old_jc = theta_vec
new_jc = theta_vec + delta_theta_small.T
true_T = self.FK(old_jc)[0]
new_T = self.FK(new_jc)[0]
true_quat = quaternion_from_matrix(true_T[:3,:3])
new_quat = quaternion_from_matrix(new_T[:3,:3])
pos_error = np.sqrt(np.sum(np.square(new_T[:3,3] - true_T[:3,3])))
ori_error = 2 * np.arccos(np.abs(np.inner(new_quat, true_quat)))
cart_error_j = np.dot(J, delta_theta_small)
pos_error_j = np.sqrt(np.sum(np.square(cart_error_j[:3])))
ori_error_j = np.sqrt(np.sum(np.square(cart_error_j[3:])))
threshold = 1e-7
assert np.abs(pos_error - pos_error_j) < threshold, "Jacobian test failed"
assert np.abs(ori_error - ori_error_j) < threshold, "Jacobian test failed"
def FK(self, thetas):
""" Wrapper for the Forward Kinematic Model which transforms joint configurations
into cartesian space.
Params:
thetas: nx6-matrix or -tensor: angles of the joints (rad)
rows: samples
columns: joints
Returns:
Ts: nx4x4-matrix: homogenous transformation matrices
n: number of samples
X[i,:,:]: homogenous transformation matrix of the ith sample
representing the end effector pose
"""
sess = tf.Session()
with sess.as_default():
thetas_T = tf.convert_to_tensor(thetas)
Ts = self.FK_tensor(thetas_T).eval()
return Ts
def FK_tensor(self, thetas):
""" Forward Kinematic Model:
Transforms joint configurations into 3D cartesian space.
Params:
thetas: nx6-tensor: angles of the joints (rad)
rows: samples
columns: joints
Returns:
_0_T_tcp: nx4x4-tensor: homogenous transformation matrices
n: number of samples
X[i,:,:]: homogenous transformation matrix of the ith sample
representing the end effector pose
"""
#make sure the datatypes are all float64
thetas = tf.cast(thetas, dtype=tf.float64)
#convert pre-filled transformation matrices to tensor
trans = tf.reshape(tf.convert_to_tensor(self.joint_transformations_notheta[:6], dtype=tf.float64), [1,6,4,4]) #1x6x4x4
trans_tcp = tf.convert_to_tensor(self.joint_transformations_notheta[6], dtype=tf.float64) #4x4
#calculate masks
Sth = tf.sin(thetas) #nx6
Cth = tf.cos(thetas) #nx6
t1 = tf.stack([Cth, -Sth], axis=-1) #nx6x2
t2 = tf.stack([Sth, Cth], axis=-1) #nx6x2
t3 = tf.stack([Sth, Cth], axis=-1) #nx6x2
t4 = tf.ones_like(t3) #nx6x2
t5 = tf.stack([t1, t2, t3, t4], axis=2) #nx6x4x2
t6 = tf.ones_like(t5) #nx6x4x2
masks = tf.concat([t5, t6], axis=-1) #nx6x4x4
_i_T_ip1 = tf.multiply(masks, trans)
_0_T_2 = tf.matmul(_i_T_ip1[:,0,:,:], _i_T_ip1[:,1,:,:]) #nx4x4
_2_T_4 = tf.matmul(_i_T_ip1[:,2,:,:], _i_T_ip1[:,3,:,:]) #nx4x4
_4_T_6 = tf.reshape(tf.matmul(_i_T_ip1[:,4,:,:], _i_T_ip1[:,5,:,:]), [-1, 4]) #(4n)x4
_0_T_4 = tf.matmul(_0_T_2, _2_T_4) #nx4x4
_4_T_tcp = tf.reshape(tf.matmul(_4_T_6, trans_tcp), [-1, 4, 4]) #nx4x4
_0_T_tcp = tf.matmul(_0_T_4, _4_T_tcp) #nx4x4
return _0_T_tcp
def IK(self, poses):
""" Analytical Inverse Kinematic Model:
Transforms end effector poses from 3D cartesian space into the joint space.
Params:
poses: nx4x4-matrix: End effector poses in 3D cartesian space
n: number of samples
poses[i,:,:]: homogenous transformation matrix of the ith sample
representing the end effector pose
Returns:
thetas: nx8x6-matrix: All 8 IK solutions
first dim: n samples
second dim: 8 solutions
third dim: 6 joint angles
"""
#rename important variables to improve readability
n = poses.shape[0]
l1 = self.tm.transforms[('link2','link1')][2,3]
l2 = self.tm.transforms[('link3','link2')][0,3]
l3 = self.tm.transforms[('link4','link3')][1,3]
l4 = self.tm.transforms[('tcp','link6')][2,3]
#convert poses from Base_T_tcp (input format) to 0_T_6 (the latter has the easier analytical solution)
#0_T_6 = 0_T_Base * Base_T_tcp * tcp_T_6
_0_T_base = np.eye(4)
_0_T_base[2,3] = -l1
tcp_T_6 = np.eye(4)
tcp_T_6[2,3] = -l4
new_poses = np.zeros_like(poses)
for i in range(n):
new_poses[i,:,:] = _0_T_base @ poses[i,:,:] @ tcp_T_6
#analytical solution from "Modeling and Control of Manipulators, Part I: Geometric and Kinematic Models" by W. Khalil
#the maximum number of solutions is eight
thetas = np.zeros((n,8,6))
sample_times = []
for i in range(n):
begin = timer()
#rename important variables to improve readability
Px = new_poses[i,0,3]
Py = new_poses[i,1,3]
Pz = new_poses[i,2,3]
sx = new_poses[i,0,0]
sy = new_poses[i,1,0]
sz = new_poses[i,2,0]
nx = new_poses[i,0,1]
ny = new_poses[i,1,1]
nz = new_poses[i,2,1]
ax = new_poses[i,0,2]
ay = new_poses[i,1,2]
az = new_poses[i,2,2]
#position equations
#two solutions for the first joint angle
thetas[i,:4,0] = math.atan2(Py, Px)
if thetas[i,0,0] < 0:
thetas[i,4:,0] = thetas[i,:4,0] + math.pi
else:
thetas[i,4:,0] = thetas[i,:4,0] - math.pi
#two more solutions for the second joint angle
for j in range(0,8,4):
B1 = Px * math.cos(thetas[i,j,0]) + Py * math.sin(thetas[i,j,0])
X = -2 * Pz * l2
Y = -2 * B1 * l2
Z = l3**2 - l2**2 - Pz**2 - B1**2
sqroot_term = X**2 + Y**2 - Z**2
if sqroot_term < 0:
#in this case, the current solution branch offers no solution
thetas[i,j:j+4,:] = np.NaN
continue
sqroot = math.sqrt(sqroot_term)
denom = X**2 + Y**2
C2_1 = (Y*Z - X*sqroot) / denom
S2_1 = (X*Z + Y*sqroot) / denom
thetas[i,j:j+2,1] = math.atan2(S2_1, C2_1)
C2_2 = (Y*Z + X*sqroot) / denom
S2_2 = (X*Z - Y*sqroot) / denom
thetas[i,j+2:j+4,1] = math.atan2(S2_2, C2_2)
#one solution for the third joint angle
S3_1 = (- Pz * S2_1 - B1 * C2_1 + l2) / l3
C3_1 = (- B1 * S2_1 + Pz * C2_1) / l3
thetas[i,j:j+2,2] = math.atan2(S3_1, C3_1)
S3_2 = (- Pz * S2_2 - B1 * C2_2 + l2) / l3
C3_2 = (- B1 * S2_2 + Pz * C2_2) / l3
thetas[i,j+2:j+4,2] = math.atan2(S3_2, C3_2)
#orientation equations
for j in range(0,8,2):
if np.isnan(thetas[i,j,0]):
continue
#two more solutions for the fourth joint angle
S1 = math.sin(thetas[i,j,0])
C1 = math.cos(thetas[i,j,0])
theta23 = thetas[i,j,1] + thetas[i,j,2]
S23 = math.sin(theta23)
C23 = math.cos(theta23)
Hx = C23 * (C1 * ax + S1 * ay) + S23 * az
Hz = S1 * ax - C1 * ay
thetas[i,j,3] = math.atan2(Hz, -Hx)
if thetas[i,j,3] < 0:
thetas[i,j+1,3] = thetas[i,j,3] + math.pi
else:
thetas[i,j+1,3] = thetas[i,j,3] - math.pi
#one solution for the fifth joint angle
Hy = - S23 * (C1 * ax + S1 * ay) + C23 * az
S4_1 = math.sin(thetas[i,j,3])
C4_1 = math.cos(thetas[i,j,3])
S4_2 = math.sin(thetas[i,j+1,3])
C4_2 = math.cos(thetas[i,j+1,3])
S5_1 = S4_1 * Hz - C4_1 * Hx
S5_2 = S4_2 * Hz - C4_2 * Hx
C5 = Hy
thetas[i,j,4] = math.atan2(S5_1, C5)
thetas[i,j+1,4] = math.atan2(S5_2, C5)
#one solution for the sixth joint angle
Fx = C23 * (C1 * sx + S1 * sy) + S23 * sz
Fz = S1 * sx - C1 * sy
Gx = C23 * (C1 * nx + S1 * ny) + S23 * nz
Gz = S1 * nx - C1 * ny
S6_1 = -C4_1 * Fz - S4_1 * Fx
C6_1 = -C4_1 * Gz - S4_1 * Gx
S6_2 = -C4_2 * Fz - S4_2 * Fx
C6_2 = -C4_2 * Gz - S4_2 * Gx
thetas[i,j,5] = math.atan2(S6_1, C6_1)
thetas[i,j+1,5] = math.atan2(S6_2, C6_2)
end = timer()
time_sample = end - begin
sample_times.append(time_sample)
print("Sum analytical IK: "+str(np.sum(sample_times))+" seconds")
print("Mean analytical IK: "+str(np.mean(sample_times))+" seconds")
print("Std analytical IK: "+str(np.std(sample_times))+" seconds")
return thetas
def plot_configurations(self, joint_configurations):
""" Plots manipulator configurations in 3D cartesian space.
Params:
joint_configurations: nx6-matrix: angles of the joints (rad)
n: number of samples
Returns:
the figure and axis references
"""
for i in range(joint_configurations.shape[0]):
joint_names = self.sorted_joint_names
joint_angles = joint_configurations[i,:]
for name, angle in zip(joint_names, joint_angles):
self.tm.set_joint(name, angle)
if i == 0:
ax = self.tm.plot_frames_in("compi", s=0.05, show_name=False)
else:
ax = self.tm.plot_frames_in("compi", s=0.05, ax=ax, show_name=False)
ax = self.tm.plot_connections_in("compi", ax=ax)
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_zlabel('z [m]')
fig = plt.gcf()
return fig, ax