def test_revolute_ctor():
q = [4.5, 1.7, 0.5, 2.1, 0.1, 2.1]
config = Configuration.from_revolute_values(q)
assert config.types == [Joint.REVOLUTE] * 6
config = Configuration.from_revolute_values([pi / 2, 0., 0.])
assert to_degrees(config.values) == [90, 0, 0]
def test_scale():
values = [1.5, 1., 2.]
types = [Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR]
config = Configuration(values, types)
config.scale(1000.)
assert config.values == [1.5, 1000., 2000.]
def test_config_merge():
config = Configuration(values=[1, 2, 3],
types=[Joint.REVOLUTE] * 3,
joint_names=['a', 'b', 'c'])
other_config = Configuration(values=[3, 2, 0],
types=[Joint.REVOLUTE] * 3,
joint_names=['a', 'b', 'd'])
config.merge(other_config)
assert config.joint_dict == {'a': 3, 'b': 2, 'c': 3, 'd': 0}
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)
def to_rlf_robot_full_conf(robot11_confval, robot12_confval, scale=1e-3):
# convert to full configuration of the RFL robot
robot21_confval = [38000, 0, -4915, 0, 0, 0, 0, 0, 0]
robot22_confval = [-12237, -4915, 0, 0, 0, 0, 0, 0]
return Configuration(
values=(
*[robot11_confval[i] * scale for i in range(0, 3)],
*[rad(robot11_confval[i]) for i in range(3, 9)],
*[robot12_confval[i] * scale for i in range(0, 2)],
*[rad(robot12_confval[i]) for i in range(2, 8)],
# below fixed
*[robot21_confval[i] * 1e-3 for i in range(0, 3)],
*[rad(robot21_confval[i]) for i in range(3, 9)],
*[robot22_confval[i] * 1e-3 for i in range(0, 2)],
*[rad(robot22_confval[i]) for i in range(2, 8)],
),
types=(2, 2, 2, 0, 0, 0, 0, 0, 0, 2, 2, 0, 0, 0, 0, 0, 0, 2, 2, 2, 0,
0, 0, 0, 0, 0, 2, 2, 0, 0, 0, 0, 0, 0),
joint_names=('bridge1_joint_EA_X', 'robot11_joint_EA_Y',
'robot11_joint_EA_Z', 'robot11_joint_1',
'robot11_joint_2', 'robot11_joint_3', 'robot11_joint_4',
'robot11_joint_5', 'robot11_joint_6',
'robot12_joint_EA_Y', 'robot12_joint_EA_Z',
'robot12_joint_1', 'robot12_joint_2', 'robot12_joint_3',
'robot12_joint_4', 'robot12_joint_5', 'robot12_joint_6',
'bridge2_joint_EA_X', 'robot21_joint_EA_Y',
'robot21_joint_EA_Z', 'robot21_joint_1',
'robot21_joint_2', 'robot21_joint_3', 'robot21_joint_4',
'robot21_joint_5', 'robot21_joint_6',
'robot22_joint_EA_Y', 'robot22_joint_EA_Z',
'robot22_joint_1', 'robot22_joint_2', 'robot22_joint_3',
'robot22_joint_4', 'robot22_joint_5', 'robot22_joint_6'))
def test_to_data():
config = Configuration.from_prismatic_and_revolute_values(
[8.312], [1.5, 0., 0., 0., 1., 0.8])
# types=[Joint.PRISMATIC, Joint.REVOLUTE]))
data = config.to_data()
assert data['values'] == [8.312, 1.5, 0., 0., 0., 1., 0.8]
assert data['types'] == [Joint.PRISMATIC] + [Joint.REVOLUTE] * 6
def compas_fab_ik_fn(frame_rcf, sampled=[]):
tool_pose = pose_from_frame(frame_rcf)
conf_vals = compute_inverse_kinematics(ikfast_ik_fn,
tool_pose,
sampled=sampled)
return [
Configuration.from_revolute_values(q) for q in conf_vals
if q is not None and len(q) > 0
]
def from_joint_trajectory(
cls, joint_trajectory
): # type: (JointTrajectory) -> MinimalTrajectory
"""Construct a instance from a :class:`compas_fab.robots.JointTrajectory`."""
conf_list = [
Configuration.from_revolute_values(pt.values)
for pt in joint_trajectory.points
]
return cls(conf_list)
def calculate_configurations_for_path(frames, robot, current_positions=[]):
"""Calculate possible configurations for a path.
Args:
frames (Frame): the path described with frames
Returns:
configurations: list of list of float
"""
configurations = []
for i, frame in enumerate(frames):
configs = robot.inverse_kinematics(frame)
qsols = [c.values for c in configs]
if not len(qsols):
return []
if i == 0:
if len(current_positions):
qsols_formatted = []
for jp_a in qsols:
jp_a_formatted = format_joint_positions(
jp_a, current_positions)
qsols_formatted.append(jp_a_formatted)
configurations.append(qsols_formatted)
else:
configurations.append(qsols)
else:
previous_qsols = configurations[-1][:]
qsols_sorted = []
for jp_b in previous_qsols:
diffs = []
qsols_formatted = []
for jp_a in qsols:
jp_a_formatted = format_joint_positions(jp_a, jp_b)
qsols_formatted.append(jp_a_formatted)
diffs.append(
sum([
math.fabs(qa - qb)
for qa, qb in zip(jp_a_formatted, jp_b)
]))
selected_idx = diffs.index(min(diffs))
qsols_sorted.append(qsols_formatted[selected_idx])
configurations.append(qsols_sorted)
print(len(configurations))
print(len(configurations[0]))
configurations = list(zip(*configurations))
print(len(configurations))
for i in range(len(configurations)):
configurations[i] = list(configurations[i])
for j, q in enumerate(configurations[i]):
configurations[i][j] = Configuration.from_revolute_values(q)
return configurations
def plan_moving_and_placing_motion(robot, element, start_configuration, tolerance_vector, safelevel_vector, attached_element_mesh):
"""Returns two trajectories for moving and placing an element.
Parameters
----------
robot : :class:`compas.robots.Robot`
element : :class:`Element`
start_configuration : :class:`Configuration`
tolerance_vector : :class:`Vector`
safelevel_vector : :class:`Vector`
attached_element_mesh : :class:`AttachedCollisionMesh`
Returns
-------
list of :class:`JointTrajectory`
"""
tolerance_position = 0.001
tolerance_axes = [math.radians(1)] * 3
target_frame = element._tool_frame.copy()
target_frame.point += tolerance_vector
safelevel_target_frame = target_frame.copy()
safelevel_target_frame.point += safelevel_vector
# Calculate goal constraints
safelevel_target_frame_tool0 = robot.from_tcf_to_t0cf(
[safelevel_target_frame])[0]
goal_constraints = robot.constraints_from_frame(safelevel_target_frame_tool0,
tolerance_position,
tolerance_axes)
moving_trajectory = robot.plan_motion(goal_constraints,
start_configuration,
options=dict(
planner_id='SBL',
attached_collision_meshes=[attached_element_mesh],
num_planning_attempts=20,
allowed_planning_time=10
))
frames = [safelevel_target_frame, target_frame]
frames_tool0 = robot.from_tcf_to_t0cf(frames)
# as start configuration take last trajectory's end configuration
last_configuration = Configuration(moving_trajectory.points[-1].values, moving_trajectory.points[-1].types)
placing_trajectory = robot.plan_cartesian_motion(frames_tool0,
last_configuration,
options=dict(
max_step=0.01,
attached_collision_meshes=[attached_element_mesh]
))
return moving_trajectory, placing_trajectory
def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = response.fraction
trajectory.points = convert_trajectory_points(
response.solution.joint_trajectory.points, joint_types)
trajectory.start_configuration = Configuration(
response.start_state.joint_state.position,
start_configuration.types)
callback(trajectory)
def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = 1.
trajectory.points = convert_trajectory_points(
response.trajectory.joint_trajectory.points, joint_types)
trajectory.start_configuration = Configuration(
response.trajectory_start.joint_state.position,
start_configuration.types)
trajectory.planning_time = response.planning_time
callback(trajectory)
def configuration_from_json():
# define filepath
filename = "assignment_03.json"
directory = "C:/Users/trtku/OneDrive/Data/03_MAS/01_github/compas_fs2021/compas_2_local/lecture_04/ko_tsuruta"
filepath = os.path.join(directory, filename)
# translate json into deserialisable data
desirial_data = read_data_from_json(filepath)
# get configurations from json
desirial_configs = [
Configuration.from_data(data) for data in desirial_data
]
return desirial_configs
def inverse_kinematics(self, tool0_frame_RCS):
"""Inverse kinematics function.
Args:
tool0_frame_RCS (:class:`Frame`): The tool0 frame to reach in robot
coordinate system (RCS).
Returns:
configurations (:obj:`list` of :class:`Configuration`): A list
of possible configurations.
"""
solutions = inverse_kinematics(tool0_frame_RCS, self.params)
configurations = []
for joint_values in solutions:
configurations.append(
Configuration.from_revolute_values(joint_values))
return configurations
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 = robot.get_joint_types_by_names(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 = robot.get_joint_types_by_names(start_state.name)
trajectory.start_configuration = Configuration(start_state.position, start_state_types)
callback(trajectory)
def __init__(
self,
rob_conf: confuse.AttrDict,
pick_station: PickStation,
ros_port: int = 9090,
):
super().__init__(ros_port=ros_port)
self.rob_conf = rob_conf
self.pick_place_tool = rob_conf.tools.get("pick_place")
self.wobjs = rob_conf.wobjs
self.global_speed_accel = rob_conf.robot_movement.global_speed_accel
self.joint_positions = rob_conf.robot_movement.joint_positions
self.speed = rob_conf.robot_movement.speed
self.zone = rob_conf.robot_movement.zone
self.docker_cfg = rob_conf.docker
# Setup pick station
self.pick_station = pick_station
if hasattr(self.joint_positions, "travel_trajectory"):
joint_positions = [
to_radians(jp) for jp in self.joint_positions.travel_trajectory
]
configurations = [
Configuration.from_revolute_values(jps)
for jps in joint_positions
]
trajectory = MinimalTrajectory(configurations)
trajectories = MinimalTrajectories([trajectory])
self.default_travel_trajectories = trajectories
self.default_return_travel_trajectories = (
trajectories.reversed_recursively())
log.debug(
f"default_travel_trajectories: {self.default_travel_trajectories}"
)
def convert_to_trajectory(response):
try:
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = response.fraction
trajectory.joint_names = response.solution.joint_trajectory.joint_names
joint_types = [joint_type_by_name[name] for name in trajectory.joint_names]
trajectory.points = convert_trajectory_points(
response.solution.joint_trajectory.points, joint_types)
start_state = response.start_state.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)
callback(trajectory)
except Exception as e:
errback(e)
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)
loader = RosFileServerLoader(client)
urdf = loader.load_urdf()
srdf = loader.load_srdf()
model = RobotModel.from_urdf_string(urdf)
semantics = RobotSemantics.from_srdf_string(srdf, model)
robot = Robot(model, semantics=semantics, client=client)
group = robot.main_group_name
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.667, -0.298, 0.336, -2.333, -1.787, 2.123, 0.571))
# 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,
planner_id='RRT')
print("Computed kinematic path with %d configurations." %
len(trajectory.points))
print(
"Executing this path at full speed would take approx. %.3f seconds." %
trajectory.time_from_start)
def inverse_kinematics(self,
robot,
frame_WCF,
start_configuration=None,
group=None,
options=None):
"""Calculate the robot's inverse kinematic for a given frame.
Parameters
----------
robot : :class:`compas_fab.robots.Robot`
The robot instance for which inverse kinematics is being calculated.
frame_WCF: :class:`compas.geometry.Frame`
The frame to calculate the inverse for.
start_configuration: :class:`compas_fab.robots.Configuration`, optional
If passed, the inverse will be calculated such that the calculated
joint positions differ the least from the start_configuration.
Defaults to the init configuration.
group: str, optional
The planning group used for calculation. Defaults to the robot's
main planning group.
options: dict, optional
Dictionary containing the following key-value pairs:
- ``"base_link"``: (:obj:`str`) Name of the base link.
- ``"avoid_collisions"``: (:obj:`bool`, optional) Whether or not to avoid collisions.
Defaults to `True`.
- ``"constraints"``: (:obj:`list` of :class:`compas_fab.robots.Constraint`, optional)
A set of constraints that the request must obey. Defaults to `None`.
- ``"attempts"``: (:obj:`int`, optional) The maximum number of inverse kinematic attempts.
Defaults to `8`.
- ``"attached_collision_meshes"``: (:obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh`, optional)
Defaults to `None`.
- ``"return_all"``: (bool, optional) return a list of all computed ik solutions
Defaults to False
Raises
------
compas_fab.backends.exceptions.BackendError
If no configuration can be found.
Returns
-------
:class:`compas_fab.robots.Configuration`
The planning group's configuration.
"""
max_iterations = is_valid_option(options, 'attempts', 8)
avoid_collisions = is_valid_option(options, 'avoid_collisions', True)
return_all = is_valid_option(options, 'return_all', False)
custom_limits = options.get('custom_limits') or {}
# return_closest_to_start = is_valid_option(options, 'return_closest_to_start', False)
# cull = is_valid_option(options, 'cull', True)
robot_uid = robot.attributes['pybullet_uid']
ik_joint_names = robot.get_configurable_joint_names(group=group)
ik_joints = joints_from_names(robot_uid, ik_joint_names)
tool_link_name = robot.get_end_effector_link_name(group=group)
tool_link = link_from_name(robot_uid, tool_link_name)
pb_custom_limits = get_custom_limits(
robot_uid,
ik_joints,
custom_limits={
joint_from_name(robot_uid, jn): lims
for jn, lims in custom_limits.items()
})
target_pose = pose_from_frame(frame_WCF)
with WorldSaver():
if start_configuration is not None:
start_conf_vals = start_configuration.values
set_joint_positions(robot_uid, ik_joints, start_conf_vals)
# else:
# sample_fn = get_sample_fn(robot_uid, ik_joints)
# start_conf_vals = sample_fn()
# if group not in self.client.planner.ik_fn_from_group:
# use default ik fn
conf_vals = self._compute_ik(robot_uid,
ik_joints,
tool_link,
target_pose,
max_iterations,
custom_limits=pb_custom_limits)
joint_types = robot.get_joint_types_by_names(ik_joint_names)
configurations = [Configuration(values=conf_val, types=joint_types, joint_names=ik_joint_names) \
for conf_val in conf_vals if conf_val is not None]
# else:
# # qs = client.inverse_kinematics(frame_WCF, group=move_group)
# configurations = self.client.planner.ik_fn_from_group[group](frame_WCF, group=group, options=options)
if avoid_collisions:
configurations = [
conf for conf in configurations
if not self.client.check_collisions(
robot, conf, options=options)
]
if return_all:
return configurations
else:
return configurations[0] if len(configurations) > 0 else None
import math
from compas.geometry import Frame
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration
with RosClient('localhost') as client:
robot = client.load_robot()
group = robot.main_group_name
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.])
# 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='RRT'))
print("Computed kinematic path with %d configurations." %
len(trajectory.points))
print(
"Executing this path at full speed would take approx. %.3f seconds." %
trajectory.time_from_start)
def test_prismatic_revolute_ctor():
config = Configuration.from_prismatic_and_revolute_values(
[8.312], [1.5, 0., 0., 0., 1., 0.8])
assert config.values == [8.312, 1.5, 0., 0., 0., 1., 0.8]
assert config.types == [Joint.PRISMATIC] + [Joint.REVOLUTE] * 6
"""
Calculate the inverse kinematics of a robot based on the frame and a starting
configuration.
"""
from ex23_load_robot import robot
from compas.geometry import Frame
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration
robot.client = RosClient()
robot.client.run()
frame = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
start_configuration = Configuration.from_revolute_values([0] * 6)
group = "manipulator" # or robot.main_group_name
response = robot.inverse_kinematics(frame,
start_configuration,
group,
constraints=None,
attempts=50)
print(response.configuration)
robot.client.close()
robot.client.terminate()
from compas.geometry import Frame
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration
with RosClient('localhost') as client:
robot = client.load_robot()
group = robot.main_group_name
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])
trajectory = robot.plan_cartesian_motion(frames,
start_configuration,
group=group,
options=dict(
max_step=0.01,
avoid_collisions=True,
))
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)
def test_ctor():
values = [1., 3., 0.1]
types = [Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR]
config = Configuration(values, types)
assert config.values == values
assert config.types == types
import os
from compas.geometry import Frame
from compas.robots import LocalPackageMeshLoader
from compas.robots import RobotModel
from compas_fab.robots import Configuration
from ur_kinematics import inverse_kinematics_ur5
models_path = os.path.join(os.path.dirname(__file__), 'models')
loader = LocalPackageMeshLoader(models_path, 'ur_description')
model = RobotModel.from_urdf_file(loader.load_urdf('ur5.urdf'))
f = Frame((0.417, 0.191, -0.005), (-0.000, 1.000, 0.000),
(1.000, 0.000, 0.000))
f.point /= 0.001
sols = inverse_kinematics_ur5(f)
for joint_values in sols:
joint_names = model.get_configurable_joint_names()
print(Configuration.from_revolute_values(joint_values, joint_names))
def test_cast_to_str():
config = Configuration([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(values=[8.312, 1.5], types=[Joint.PRISMATIC, Joint.REVOLUTE]))
assert str(config) == 'Configuration((8.312, 1.500), (2, 0))'
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration
with RosClient() as client:
robot = client.load_robot()
assert robot.name == 'ur5'
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)
# Units:
# - Revolute joint : radiants
# - Prismatic joint: meters
import math
from compas.robots.model import Joint
from compas_fab.robots import Configuration
print('Default constructor')
print(Configuration([math.pi, 4], [Joint.REVOLUTE, Joint.PRISMATIC]))
print(
Configuration([math.pi, 4], [Joint.REVOLUTE, Joint.PRISMATIC],
['joint_1', 'ext_axis_1']))
print()
print('Construct from revolute values')
print(Configuration.from_revolute_values([math.pi, 0]))
print(Configuration.from_revolute_values([math.pi, 0], ['joint_1', 'joint_2']))
print()
print('Construct from prismatic & revolute values')
print(Configuration.from_prismatic_and_revolute_values([4], [math.pi]))
print(
Configuration.from_prismatic_and_revolute_values(
[4], [math.pi], ['ext_axis_1', 'joint_1']))
print()
print('Merge two configurations')
ext_axes = Configuration([4], [Joint.PRISMATIC], ['ext_axis_1'])
arm_joints = Configuration([math.pi], [Joint.REVOLUTE], ['joint_1'])
full_cfg = ext_axes.merged(arm_joints)