def test_multiple_visuals():
urdf = """
<robot name="robot_name">
<link name="link0">
<visual>
<origin xyz="0 0 1"/>
<geometry>
<sphere radius="0.123"/>
</geometry>
</visual>
</link>
<link name="link1">
<visual>
<origin xyz="0 0 1"/>
<geometry>
<sphere radius="0.234"/>
</geometry>
</visual>
</link>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
assert_equal(len(tm.visuals), 2)
def test_multiple_parents():
urdf = """
<?xml version="1.0"?>
<robot name="mmm">
<link name="parent0"/>
<link name="parent1"/>
<link name="child"/>
<joint name="joint0" type="revolute">
<origin xyz="1 0 0" rpy="0 0 0"/>
<parent link="parent0"/>
<child link="child"/>
<axis xyz="1 0 0"/>
</joint>
<joint name="joint1" type="revolute">
<origin xyz="0 1 0" rpy="0 0 0"/>
<parent link="parent1"/>
<child link="child"/>
<axis xyz="1 0 0"/>
</joint>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
p0c = tm.get_transform("parent0", "child")
p1c = tm.get_transform("parent1", "child")
assert_equal(p0c[0, 3], p1c[1, 3])
def test_ee_frame():
tm = UrdfTransformManager()
tm.load_urdf(COMPI_URDF)
link7_to_linkmount = tm.get_transform("link6", "linkmount")
assert_array_almost_equal(
link7_to_linkmount,
np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1.124], [0, 0, 0, 1]]))
def test_collision():
urdf = """
<robot name="robot_name">
<link name="link0"/>
<link name="link1">
<collision>
<origin xyz="0 0 1"/>
<geometry/>
</collision>
</link>
<joint name="joint0" type="fixed">
<parent link="link0"/>
<child link="link1"/>
<origin xyz="0 0 1"/>
</joint>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
link1_to_link0 = tm.get_transform("link1", "link0")
expected_link1_to_link0 = transform_from(np.eye(3), np.array([0, 0, 1]))
assert_array_almost_equal(link1_to_link0, expected_link1_to_link0)
link1_to_link0 = tm.get_transform("link1/collision_0", "link0")
expected_link1_to_link0 = transform_from(np.eye(3), np.array([0, 0, 2]))
assert_array_almost_equal(link1_to_link0, expected_link1_to_link0)
def test_fixed_joint():
tm = UrdfTransformManager()
tm.load_urdf(COMPI_URDF)
tcp_to_link0 = tm.get_transform("tcp", "linkmount")
assert_array_almost_equal(
tcp_to_link0,
np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1.174], [0, 0, 0, 1]]))
def test_joint_limits():
tm = UrdfTransformManager()
tm.load_urdf(COMPI_URDF)
assert_raises(KeyError, tm.get_joint_limits, "unknown_joint")
assert_array_almost_equal(tm.get_joint_limits("joint1"), (-1, 1))
assert_array_almost_equal(tm.get_joint_limits("joint2"), (-1, np.inf))
assert_array_almost_equal(tm.get_joint_limits("joint3"), (-np.inf, 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_without_origin():
urdf = """
<robot name="robot_name">
<link name="link0"/>
<link name="link1"/>
<joint name="joint0" type="fixed">
<parent link="link0"/>
<child link="link1"/>
</joint>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
link1_to_link0 = tm.get_transform("link1", "link0")
assert_array_almost_equal(link1_to_link0, np.eye(4))
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_collision_sphere():
urdf = """
<robot name="robot_name">
<link name="link0">
<collision>
<origin xyz="0 0 1"/>
<geometry>
<sphere radius="0.123"/>
</geometry>
</collision>
</link>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
assert_equal(len(tm.collision_objects), 1)
assert_equal(tm.collision_objects[0].radius, 0.123)
def test_collision_box_without_size():
urdf = """
<robot name="robot_name">
<link name="link0">
<collision>
<origin xyz="0 0 1"/>
<geometry>
<box/>
</geometry>
</collision>
</link>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
assert_equal(len(tm.collision_objects), 1)
assert_array_almost_equal(tm.collision_objects[0].size, np.zeros(3))
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]]))
import numpy as np
from pytransform.urdf import UrdfTransformManager
# A script that helps to determine the optimum ratio
# between step-sizes in Cartesian and joint space.
# We sample joint angles with variance 1 and measure
# the resulting standard deviation in Cartesian space.
tm = UrdfTransformManager()
tm.load_urdf(open("../data/kuka_lbr.urdf", "r"))
random_state = np.random.RandomState(42)
n_samples = 1000
joint_limits = np.array(
[tm.get_joint_limits("kuka_lbr_l_joint_%d" % (j + 1))
for j in range(7)]
)
#joint_angles = (random_state.rand(n_samples, 7) *
# (joint_limits[:, 1] - joint_limits[:, 0]) +
# joint_limits[:, 0])
joint_angles = random_state.randn(n_samples, 7)
positions = np.empty((n_samples, 3))
for n in range(n_samples):
for j in range(7):
tm.set_joint("kuka_lbr_l_joint_%d" % (j + 1), joint_angles[n, j])
positions[n] = tm.get_transform("kuka_lbr_l_link_7", "kuka_lbr_l_link_0")[:3, 3]
print(np.std(joint_angles, axis=0))
print(np.mean(positions, axis=0))
print(np.std(positions, axis=0))
class Pendulum(Environment):
"""Optimize a trajectory according to some criteria.
Parameters
----------
x0 : array-like, shape = (n_task_dims,), optional (default: [0, 0])
Initial position.
g : array-like, shape = (n_task_dims,), optional (default: [1, 1])
Goal position.
execution_time : float, optional (default: 1.0)
Execution time in seconds
dt : float, optional (default: 0.01)
Time between successive steps in seconds.
log_to_file: optional, boolean or string (default: False)
Log results to given file, it will be located in the $BL_LOG_PATH
log_to_stdout: optional, boolean (default: False)
Log to standard output
"""
def __init__(self,
x0=np.zeros(7),
g=np.zeros(7),
execution_time=1.0,
dt=0.01,
log_to_file=False,
log_to_stdout=False):
self.x0 = x0
self.g = g
self.execution_time = execution_time
self.dt = dt
self.log_to_file = log_to_file
self.log_to_stdout = log_to_stdout
def init(self):
"""Initialize environment."""
self.x0 = np.asarray(self.x0)
self.g = np.asarray(self.g)
self.n_task_dims = self.x0.shape[0]
self.logger = get_logger(self, self.log_to_file, self.log_to_stdout)
n_steps = 1 + int(self.execution_time / self.dt)
self.X = np.empty((n_steps, self.n_task_dims))
self.xd = np.empty(self.n_task_dims)
self.xdd = np.empty(self.n_task_dims)
self.tm = UrdfTransformManager()
self.tm.load_urdf(open("../data/kuka_lbr.urdf", "r"))
self.sphere_pos = np.array([0.6, 0.0, 1.0])
self.pendulum_radius = 0.5
self.sphere_radius = 0.1
# We set the target so that the trajectory has to be modified
# significantly. It has to hit the pendulum from another direction.
self.target_pos = np.array([
0.6 + self.pendulum_radius * np.cos(0.6 * np.pi),
self.pendulum_radius * np.sin(0.6 * np.pi), 1.5
])
self.pendulum_mass = 1.0
self.ee_mass = 30.0
def reset(self):
"""Reset state of the environment."""
self.t = 0
def get_num_inputs(self):
"""Get number of environment inputs.
Returns
-------
n : int
number of environment inputs
"""
return 3 * self.n_task_dims
def get_num_outputs(self):
"""Get number of environment outputs.
Returns
-------
n : int
number of environment outputs
"""
return 3 * self.n_task_dims
def get_outputs(self, values):
"""Get environment outputs.
Parameters
----------
values : array
Outputs of the environment: positions, velocities and accelerations
in that order, e.g. for n_task_dims=2 the order would be xxvvaa
"""
if self.t == 0:
values[:self.n_task_dims] = np.copy(self.x0)
values[self.n_task_dims:-self.n_task_dims] = np.zeros(
self.n_task_dims)
values[-self.n_task_dims:] = np.zeros(self.n_task_dims)
else:
values[:self.n_task_dims] = self.X[self.t - 1]
values[self.n_task_dims:-self.n_task_dims] = self.xd
values[-self.n_task_dims:] = self.xdd
def set_inputs(self, values):
"""Set environment inputs, e.g. next action.
Parameters
----------
values : array,
Inputs for the environment: positions, velocities and accelerations
in that order, e.g. for n_task_dims=2 the order would be xxvvaa
"""
if np.all(np.isfinite(values[:self.n_task_dims])):
self.X[self.t, :] = values[:self.n_task_dims]
self.xd[:] = values[self.n_task_dims:-self.n_task_dims]
self.xdd[:] = values[-self.n_task_dims:]
else:
self.X[self.t, :] = self.X[self.t - 1, :]
def step_action(self):
"""Execute step perfectly."""
self.t += 1
def is_evaluation_done(self):
"""Check if the time is over.
Returns
-------
finished : bool
Is the episode finished?
"""
return self.t * self.dt > self.execution_time
def get_feedback(self):
"""Get reward per timestamp based on weighted criteria (penalties)
Returns
-------
rewards : array-like, shape (n_steps,)
reward for every timestamp; non-positive values
"""
colliding = False
collision_velocity = None
collision_happened = False
for t in range(len(self.X)):
p = forward(self.tm, self.X[t], tips=["kuka_lbr_l_link_7"])[0]
if colliding:
collision_velocity -= p
collision_velocity /= -self.dt
collision_happened = True
break
elif np.linalg.norm(p - self.sphere_pos) < 2 * self.sphere_radius:
colliding = True
collision_velocity = p.copy()
sphere_displacement = self.sphere_pos.copy()
if collision_happened:
d = displacement(self.ee_mass, self.pendulum_mass,
self.pendulum_radius, collision_velocity)
sphere_displacement += d
distance = np.linalg.norm(sphere_displacement - self.target_pos)
return np.array([-distance**2])
def is_behavior_learning_done(self):
"""Check if the behavior learning is finished.
Returns
-------
finished : bool
Always false
"""
return False
def get_maximum_feedback(self):
"""Returns the maximum sum of feedbacks obtainable."""
return 0.0
def plot(self, ax=None):
"""Plot a two-dimensional environment.
Parameters
----------
ax : Axis, optional
Matplotlib axis
"""
if ax is None:
ax = plt.subplot(111, projection="3d")
ax.scatter(self.sphere_pos[0],
self.sphere_pos[1],
self.sphere_pos[2],
c="gray",
s=200)
ax.scatter(self.sphere_pos[0],
self.sphere_pos[1],
self.sphere_pos[2] + self.pendulum_radius,
c="k",
s=100)
ax.plot(
(self.sphere_pos[0], self.sphere_pos[0]),
(self.sphere_pos[1], self.sphere_pos[1]),
(self.sphere_pos[2] + self.pendulum_radius, self.sphere_pos[2]),
c="gray")
ax.scatter(self.target_pos[0],
self.target_pos[1],
self.target_pos[2],
c="orange",
s=100)
initial_positions = forward(self.tm, self.x0)
for p in initial_positions:
ax.scatter(p[0], p[1], p[2], c="r", s=100)
final_positions = forward(self.tm, self.g)
for p in final_positions:
ax.scatter(p[0], p[1], p[2], c="g", s=100)
P = []
colliding = False
collision_velocity = None
collision_happened = 0
for t in range(len(self.X)):
p = forward(self.tm, self.X[t])
P.append(p)
if colliding:
collision_velocity -= p[-1]
collision_velocity /= -self.dt
colliding = False
elif not collision_happened and np.linalg.norm(
p[-1] - self.sphere_pos) < 0.1:
colliding = True
collision_velocity = p[-1].copy()
collision_happened = t
P = np.array(P)
for i in range(len(LINKNAMES)):
ax.plot(P[:, i, 0], P[:, i, 1], P[:, i, 2], c="k", ls="--")
if collision_happened:
v = collision_velocity
v_norm = 0.3 * v / np.linalg.norm(v)
ax.plot([
P[collision_happened, -1, 0],
P[collision_happened, -1, 0] + v_norm[0]
], [
P[collision_happened, -1, 1],
P[collision_happened, -1, 1] + v_norm[1]
], [
P[collision_happened, -1, 2],
P[collision_happened, -1, 2] + v_norm[2]
])
d = displacement(self.ee_mass, self.pendulum_mass,
self.pendulum_radius, v)
sphere_displacement = self.sphere_pos + d
ax.scatter(sphere_displacement[0],
sphere_displacement[1],
sphere_displacement[2],
c="k",
s=200)
ax.plot((self.sphere_pos[0], sphere_displacement[0]),
(self.sphere_pos[1], sphere_displacement[1]),
(self.sphere_pos[2] + self.pendulum_radius,
sphere_displacement[2]),
c="k")
ax.set_xlim((-1.0, 1.5))
ax.set_ylim((-1.25, 1.25))
ax.set_zlim((-0.25, 2.25))
return ax
<robot name="collision">
<link name="collision">
<collision name="collision_01">
<origin xyz="0 0 1" rpy="1 0 0"/>
<geometry>
<sphere radius="0.2"/>
</geometry>
</collision>
<collision name="collision_02">
<origin xyz="0 0.5 0" rpy="0 1 0"/>
<geometry>
<cylinder radius="0.1" length="2"/>
</geometry>
</collision>
<collision name="collision_03">
<origin xyz="-0.5 0 0" rpy="0 0 1"/>
<geometry>
<box size="0.3 0.4 0.5"/>
</geometry>
</collision>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(URDF)
ax = tm.plot_frames_in("collision", s=0.1)
tm.plot_collision_objects("collision", ax)
ax.set_zlim((-0.5, 1.5))
plt.show()
def test_unknown_joint():
tm = UrdfTransformManager()
tm.load_urdf(COMPI_URDF)
assert_raises(KeyError, tm.set_joint, "unknown_joint", 0)