def test_bool():
config = Configuration(joint_values=[1, 2, 3],
joint_types=[Joint.REVOLUTE] * 3,
joint_names=['a', 'b', 'c'])
assert config
config = Configuration(joint_values=[1, 2, 3],
joint_types=[Joint.REVOLUTE] * 3)
assert config
config = Configuration()
assert config
def random_configuration(self):
"""Get a random configuration.
Returns
-------
:class:`compas.robots.Configuration`
Instance of a configuration with randomized joint values.
Note
----
No collision checking is involved, the configuration may be invalid.
"""
configurable_joints = self.get_configurable_joints()
values = []
for joint in configurable_joints:
if joint.limit:
values.append(joint.limit.lower +
(joint.limit.upper - joint.limit.lower) *
random.random())
else:
values.append(0)
joint_names = self.get_configurable_joint_names()
joint_types = self.get_joint_types_by_names(joint_names)
return Configuration(values, joint_types, joint_names)
def zero_configuration(self):
"""Get the zero joint configuration.
If zero is out of joint limits ``(upper, lower)`` then
``(upper + lower) / 2`` is used as joint value.
Examples
--------
>>> robot = RobotModel.ur5()
>>> robot.zero_configuration()
Configuration((0.000, 0.000, 0.000, 0.000, 0.000, 0.000), (0, 0, 0, 0, 0, 0), \
('shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'))
"""
values = []
joint_names = []
joint_types = []
for joint in self.get_configurable_joints():
if joint.limit and not (0 <= joint.limit.upper
and 0 >= joint.limit.lower):
values.append((joint.limit.upper + joint.limit.lower) / 2.)
else:
values.append(0)
joint_names.append(joint.name)
joint_types.append(joint.type)
return Configuration(values, joint_types, joint_names)
def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = 1.
trajectory.joint_names = response.trajectory.joint_trajectory.joint_names
trajectory.planning_time = response.planning_time
joint_types = [
joint_type_by_name[name] for name in trajectory.joint_names
]
trajectory.points = convert_trajectory_points(
response.trajectory.joint_trajectory.points, joint_types)
start_state = response.trajectory_start.joint_state
start_state_types = [
joint_type_by_name[name] for name in start_state.name
]
trajectory.start_configuration = Configuration(
start_state.position, start_state_types, start_state.name)
trajectory.attached_collision_meshes = list(
itertools.chain(*[
aco.to_attached_collision_meshes() for aco in
response.trajectory_start.attached_collision_objects
]))
callback(trajectory)
def test_configuration_deepcopy():
c = Configuration([1, 2, 3], [0, 0, 0])
c_copy = copy.deepcopy(c)
assert c is not c_copy
assert c_copy.joint_values[0] == 1
c_copy.joint_values[0] = 0
assert c_copy.joint_values[0] == 0
assert c.joint_values[0] == 1
def test_to_data():
config = Configuration.from_prismatic_and_revolute_values(
[8.312], [1.5, 0., 0., 0., 1., 0.8])
# joint_types=[Joint.PRISMATIC, Joint.REVOLUTE]))
data = config.to_data()
assert data['joint_values'] == [8.312, 1.5, 0., 0., 0., 1., 0.8]
assert data['joint_types'] == [Joint.PRISMATIC] + [Joint.REVOLUTE] * 6
def test_external_axes():
ea = rrc.ExternalAxes()
assert list(ea) == []
ea = rrc.ExternalAxes(30)
assert list(ea) == [30]
ea = rrc.ExternalAxes(30, 10, 0)
assert list(ea) == [30, 10, 0]
ea = rrc.ExternalAxes([30, 10, 0])
assert list(ea) == [30, 10, 0]
ea = rrc.ExternalAxes(iter([30, 10, 0]))
assert list(ea) == [30, 10, 0]
j = rrc.ExternalAxes(30, 45, 0)
c = j.to_configuration_primitive([0, 1, 2], ['j1', 'j2', 'j3'])
assert c.joint_values == [math.pi/6, math.pi/4, 0.0]
assert c.joint_names == ['j1', 'j2', 'j3']
j = rrc.ExternalAxes(30, -45, 100)
c = j.to_configuration_primitive([0, 1, 2])
assert c.joint_values == [math.pi/6, -math.pi/4, 0.1]
assert len(c.joint_names) == 0
config = Configuration([2*math.pi, math.pi, math.pi/2, math.pi/3, math.pi/4, math.pi/6], [0, 0, 0, 0, 0, 0])
rj = rrc.ExternalAxes.from_configuration_primitive(config)
assert allclose(rj.values, [360, 180, 90, 60, 45, 30])
config = Configuration(
[0, 2*math.pi, math.pi, math.pi/2, math.pi/3, math.pi/4, math.pi/6 ],
[0, 0, 0, 0, 0, 0, 0],
['a', 'b', 'c', 'd', 'e', 'f', 'g'])
rj = rrc.ExternalAxes.from_configuration_primitive(config, ['b', 'c', 'd', 'e', 'f', 'g'])
assert allclose(rj.values, [360, 180, 90, 60, 45, 30])
j = rrc.RobotJoints(30, 10, 0)
config = j.to_configuration_primitive([0, 0, 0])
new_j = rrc.ExternalAxes.from_configuration_primitive(config)
assert allclose(j.values, new_j.values)
def trj():
p1 = JointTrajectoryPoint([1.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.] * 6,
time_from_start=Duration(2, 1293))
p2 = JointTrajectoryPoint([0.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.] * 6,
time_from_start=Duration(6, 0))
config = Configuration.from_revolute_values([0.] * 6)
return JointTrajectory(trajectory_points=[p1, p2],
joint_names=[
'joint_1', 'joint_2', 'joint_3', 'joint_4',
'joint_5', 'joint_6'
],
start_configuration=config)
def data(self, data):
self.points = list(
map(JointTrajectoryPoint.from_data,
data.get('points') or []))
self.joint_names = data.get('joint_names', [])
if data.get('start_configuration'):
self.start_configuration = Configuration.from_data(
data.get('start_configuration'))
self.fraction = data.get('fraction')
self.attached_collision_meshes = [
AttachedCollisionMesh.from_data(acm_data)
for acm_data in data.get('attached_collision_meshes', [])
]
def test_get():
config = Configuration(joint_values=[1, 2, 0],
joint_types=[Joint.REVOLUTE] * 3,
joint_names=['a', 'b', 'c'])
assert config.get('a') == 1
assert config.get('DNE') is None
assert config.get('DNE', 5) == 5
assert config.get('c', 5) == 0
def test_joint_names():
with pytest.raises(ValueError):
config = Configuration(joint_values=[1, 2],
joint_types=[Joint.REVOLUTE] * 2,
joint_names=['a', 'a'])
config = Configuration(joint_values=[1, 2],
joint_types=[Joint.REVOLUTE] * 2,
joint_names=['a', 'b'])
with pytest.raises(ValueError):
config.joint_names = ['a', 'a']
with pytest.raises(ValueError):
config.joint_names[1] = config.joint_names[0]
def to_configuration_primitive(self, joint_types, joint_names=None):
"""Convert the RobotJoints to a :class:`compas.robots.Configuration`, including the unit conversion
from mm and degrees to meters and radians.
Parameters
----------
joint_types : :obj:`list`
List of integers representing the joint types of the corresponding internal axes values.
joint_names : :obj:`list`
List of strings representing the joint names of the corresponding internal axes values. Optional.
Returns
-------
:class:`compas.robots.Configuration`
"""
joint_values = [
_convert_unit_to_meters_radians(value, type_)
for value, type_ in zip(self.values, joint_types)
]
return Configuration(joint_values, joint_types, joint_names)
def get_config_or_pose(self, config_values_or_plane):
"""Parses multiple input data types and returns a configuration or a pose
represented as a transformation matrix.
Args:
config_values_or_plane (comma-separated :obj:`string`, or :class:`Configuration` instance,
or a Rhino :obj:`Plane`).
"""
if not config_values_or_plane:
return None
try:
return Pose.from_list(vrep_pose_from_plane(config_values_or_plane))
except (TypeError, IndexError):
try:
if config_values_or_plane.external_axes and config_values_or_plane.joint_values:
return config_values_or_plane
except AttributeError:
values = map(float, config_values_or_plane.split(','))
return Configuration.from_degrees_list(values)
def convert_trajectory(joints, solution, solution_start_state, fraction,
planning_time, source_message):
trajectory = JointTrajectory()
trajectory.source_message = source_message
trajectory.fraction = fraction
trajectory.joint_names = solution.joint_trajectory.joint_names
trajectory.planning_time = planning_time
joint_types = [joints[name] for name in trajectory.joint_names]
trajectory.points = convert_trajectory_points(
solution.joint_trajectory.points, joint_types)
start_state = solution_start_state.joint_state
start_state_types = [joints[name] for name in start_state.name]
trajectory.start_configuration = Configuration(start_state.position,
start_state_types,
start_state.name)
trajectory.attached_collision_meshes = list(
itertools.chain(*[
aco.to_attached_collision_meshes()
for aco in solution_start_state.attached_collision_objects
]))
return trajectory
def test_ctor():
values = [1., 3., 0.1]
joint_types = [Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR]
config = Configuration(values, joint_types)
assert config.joint_values == values
assert config.joint_types == joint_types
from compas.robots import Configuration
from compas_fab.robots.ur5 import Robot
robot = Robot()
configuration = Configuration.from_revolute_values(
[-2.238, -1.153, -2.174, 0.185, 0.667, 0.])
frame_WCF = robot.forward_kinematics(configuration)
print("Frame in the world coordinate system")
print(frame_WCF)
assert repr(frame_WCF.point) == 'Point(0.300, 0.100, 0.500)'
def config_from_vrep(list_of_floats, scale):
# COMPAS FAB uses radians and meters, just like V-REP,
# so in general, scale should always be 1
radians = list_of_floats[3:]
prismatic_values = map(lambda v: v * scale, list_of_floats[0:3])
return Configuration.from_prismatic_and_revolute_values(prismatic_values, radians)
import math
from compas.geometry import Frame
from compas.robots import Configuration
from compas_fab.backends import RosClient
with RosClient() as client:
robot = client.load_robot()
assert robot.name == 'ur5'
frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
tolerance_position = 0.001
tolerance_axes = [math.radians(1)] * 3
start_configuration = Configuration.from_revolute_values(
[-0.042, 4.295, 0, -3.327, 4.755, 0.])
group = robot.main_group_name
# create goal constraints from frame
goal_constraints = robot.constraints_from_frame(frame, tolerance_position,
tolerance_axes, group)
trajectory = robot.plan_motion(
goal_constraints,
start_configuration,
group,
options=dict(planner_id='RRTstarkConfigDefault'))
print("Computed kinematic path with %d configurations." %
len(trajectory.points))
print(
"Executing this path at full speed would take approx. %.3f seconds." %
def test_cast_to_str():
config = Configuration([math.pi / 2, 3., 0.1],
[Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR])
assert str(config) == 'Configuration((1.571, 3.000, 0.100), (0, 2, 5))'
def test_from_data():
config = Configuration.from_data(
dict(joint_values=[8.312, 1.5],
joint_types=[Joint.PRISMATIC, Joint.REVOLUTE]))
assert str(config) == 'Configuration((8.312, 1.500), (2, 0))'
def joint_angles_to_configurations(robot, solutions, group=None):
joint_names = robot.get_configurable_joint_names(group=group)
return [
Configuration.from_revolute_values(q, joint_names=joint_names)
if q else None for q in solutions
]
from compas_fab.robots import CollisionMesh, AttachedCollisionMesh
with RosClient() as client:
robot = client.load_robot()
assert robot.name == 'ur5'
ee_link_name = robot.get_end_effector_link_name()
mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl'))
cm = CollisionMesh(mesh, 'tip')
acm = AttachedCollisionMesh(cm, link_name=ee_link_name)
client.add_attached_collision_mesh(acm)
frames = []
frames.append(Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]))
frames.append(Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0]))
start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000])
options = {
'max_step': 0.01,
'avoid_collisions': True,
}
trajectory = robot.plan_cartesian_motion(frames,
start_configuration,
options=options)
print("Computed cartesian path with %d configurations, " % len(trajectory.points))
print("following %d%% of requested trajectory." % (trajectory.fraction * 100))
print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)
print("Path plan computed with the attached collision meshes: {}".format(
[acm.collision_mesh.id for acm in trajectory.attached_collision_meshes]))
def test_prismatic_revolute_ctor():
config = Configuration.from_prismatic_and_revolute_values(
[8.312], [1.5, 0., 0., 0., 1., 0.8])
assert config.joint_values == [8.312, 1.5, 0., 0., 0., 1., 0.8]
assert config.joint_types == [Joint.PRISMATIC] + [Joint.REVOLUTE] * 6
