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 jtp():
return JointTrajectoryPoint([1.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.] * 6,
time_from_start=Duration(2, 1293),
joint_names=[
'joint_1', 'joint_2', 'joint_3', 'joint_4',
'joint_5', 'joint_6'
])
def inverse_kinematics_async(self,
callback,
errback,
frame_WCF,
start_configuration=None,
group=None,
options=None):
"""Asynchronous handler of MoveIt IK service."""
base_link = options['base_link']
header = Header(frame_id=base_link)
pose_stamped = PoseStamped(header, Pose.from_frame(frame_WCF))
joint_state = JointState(name=start_configuration.joint_names,
position=start_configuration.joint_values,
header=header)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if options.get('attached_collision_meshes'):
for acm in options['attached_collision_meshes']:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
constraints = convert_constraints_to_rosmsg(options.get('constraints'),
header)
timeout_in_secs = options.get('timeout', 2)
timeout_duration = Duration(timeout_in_secs, 0).to_data()
ik_request = PositionIKRequest(group_name=group,
robot_state=start_state,
constraints=constraints,
pose_stamped=pose_stamped,
avoid_collisions=options.get(
'avoid_collisions', True),
attempts=options.get('attempts', 8),
timeout=timeout_duration)
# The field `attempts` was removed in Noetic (and higher)
# so it needs to be removed from the message otherwise it causes a serialization error
# https://github.com/ros-planning/moveit/pull/1288
if self.ros_client.ros_distro not in (RosDistro.KINETIC,
RosDistro.MELODIC):
del ik_request.attempts
def convert_to_positions(response):
callback((response.solution.joint_state.position,
response.solution.joint_state.name))
self.GET_POSITION_IK(self.ros_client, (ik_request, ),
convert_to_positions, errback)
def convert_trajectory_points(points, joint_types):
result = []
for pt in points:
jtp = JointTrajectoryPoint(joint_values=pt.positions,
joint_types=joint_types,
velocities=pt.velocities,
accelerations=pt.accelerations,
effort=pt.effort,
time_from_start=Duration(pt.time_from_start.secs, pt.time_from_start.nsecs))
result.append(jtp)
return result
def plan_motion(self, robot, goal_constraints, start_configuration=None, group=None, options=None):
"""Calculates a motion path.
Parameters
----------
robot : :class:`compas_fab.robots.Robot`
The robot instance for which the motion path is being calculated.
goal_constraints: list of :class:`compas_fab.robots.Constraint`
The goal to be achieved, defined in a set of constraints.
Constraints can be very specific, for example defining value domains
for each joint, such that the goal configuration is included,
or defining a volume in space, to which a specific robot link (e.g.
the end-effector) is required to move to.
start_configuration: :class:`compas_fab.robots.Configuration`, optional
The robot's full configuration, i.e. values for all configurable
joints of the entire robot, at the starting position.
group: str, optional
The name of the group to plan for.
options : dict, optional
Dictionary containing kwargs for arguments specific to
the client being queried.
- ``"avoid_collisions"``: (:obj:`bool`, optional)
Whether or not to avoid collisions. Defaults to ``True``.
- ``"max_step"``: (:obj:`float`, optional) The approximate distance between the
calculated points. (Defined in the robot's units.) Defaults to ``0.01``.
- ``"jump_threshold"``: (:obj:`float`, optional)
The maximum allowed distance of joint positions between consecutive
points. If the distance is found to be above this threshold, the
path computation fails. It must be specified in relation to max_step.
If this threshold is ``0``, 'jumps' might occur, resulting in an invalid
cartesian path. Defaults to :math:`\\pi / 2`.
Returns
-------
:class:`compas_fab.robots.JointTrajectory`
The calculated trajectory.
"""
robot_uid = robot.attributes['pybullet_uid']
# * parse options
verbose = is_valid_option(options, 'verbose', False)
diagnosis = options.get('diagnosis', False)
custom_limits = options.get('custom_limits') or {}
resolutions = options.get('resolutions') or 0.1
weights = options.get('weights') or None
rrt_restarts = options.get('rrt_restarts', 2)
rrt_iterations = options.get('rrt_iterations', 20)
smooth_iterations = options.get('smooth_iterations', 20)
# TODO: auto compute joint weight
# print('plan motion options: ', options)
# * convert link/joint names to pybullet indices
joint_names = robot.get_configurable_joint_names(group=group)
ik_joints = joints_from_names(robot_uid, joint_names)
joint_types = robot.get_joint_types_by_names(joint_names)
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()})
# print('pb custom limits: ', list(pb_custom_limits))
with WorldSaver():
if start_configuration is not None:
# * set to start conf
self.client.set_robot_configuration(robot, start_configuration)
sample_fn = get_sample_fn(robot_uid, ik_joints, custom_limits=pb_custom_limits)
distance_fn = get_distance_fn(robot_uid, ik_joints, weights=weights)
extend_fn = get_extend_fn(robot_uid, ik_joints, resolutions=resolutions)
options['robot'] = robot
collision_fn = PyChoreoConfigurationCollisionChecker(self.client)._get_collision_fn(robot, joint_names, options=options)
start_conf = get_joint_positions(robot_uid, ik_joints)
end_conf = self._joint_values_from_joint_constraints(joint_names, goal_constraints)
assert len(ik_joints) == len(end_conf)
if not check_initial_end(start_conf, end_conf, collision_fn, diagnosis=diagnosis):
return None
path = birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn, collision_fn,
restarts=rrt_restarts, iterations=rrt_iterations, smooth=smooth_iterations)
#return plan_lazy_prm(start_conf, end_conf, sample_fn, extend_fn, collision_fn)
if path is None:
# TODO use LOG
if verbose:
cprint('No free motion found!', 'red')
return None
else:
jt_traj_pts = []
for i, conf in enumerate(path):
# c_conf = Configuration(values=conf, types=joint_types, joint_names=joint_names)
jt_traj_pt = JointTrajectoryPoint(values=conf, types=joint_types, time_from_start=Duration(i*1,0))
# TODO why don't we have a `joint_names` input for JointTrajectoryPoint?
# https://github.com/compas-dev/compas_fab/blob/master/src/compas_fab/robots/trajectory.py#L64
jt_traj_pt.joint_names = joint_names
jt_traj_pts.append(jt_traj_pt)
trajectory = JointTrajectory(trajectory_points=jt_traj_pts,
joint_names=joint_names, start_configuration=jt_traj_pts[0], fraction=1.0)
return trajectory
def plan_cartesian_motion(self,
robot,
frames_WCF,
start_configuration=None,
group=None,
options=None):
"""Calculates a cartesian motion path (linear in tool space).
Parameters
----------
robot : :class:`compas_fab.robots.Robot`
The robot instance for which the motion path is being calculated.
frames_WCF: list of :class:`compas.geometry.Frame`
The frames through which the path is defined.
start_configuration: :class:`Configuration`, optional
The robot's full configuration, i.e. values for all configurable
joints of the entire robot, at the starting position.
group: str, optional
The planning group used for calculation.
options: dict, optional
Dictionary containing kwargs for arguments specific to
the client being queried.
- ``"diagnosis"``: (:obj:`bool`, optional)
. Defaults to ``False``.
- ``"avoid_collisions"``: (:obj:`bool`, optional)
Whether or not to avoid collisions. Defaults to ``True``.
- ``"planner_id"``: (:obj:`str`)
The name of the algorithm used for path planning.
Defaults to ``'IterativeIK'``.
Available planners: ``'IterativeIK', 'LadderGraph'``
- ``"max_step"``: (:obj:`float`, optional) The approximate distance between the
calculated points. (Defined in the robot's units.) Defaults to ``0.01``.
- ``"jump_threshold"``: (:obj:`float`, optional)
The maximum allowed distance of joint positions between consecutive
points. If the distance is found to be above this threshold, the
path computation fails. It must be specified in relation to max_step.
If this threshold is ``0``, 'jumps' might occur, resulting in an invalid
cartesian path.
Defaults to :math:`\\pi / 6` for revolute or continuous joints, 0.1 for prismatic joints.
# TODO: JointConstraint
Returns
-------
:class:`compas_fab.robots.JointTrajectory`
The calculated trajectory.
"""
robot_uid = robot.attributes['pybullet_uid']
# * convert link/joint names to pybullet indices
base_link_name = robot.get_base_link_name(group=group)
tool_link_name = robot.get_end_effector_link_name(group=group)
tool_link = link_from_name(robot_uid, tool_link_name)
base_link = link_from_name(robot_uid, base_link_name)
joint_names = robot.get_configurable_joint_names(group=group)
joint_types = robot.get_joint_types_by_names(joint_names)
ik_joints = joints_from_names(robot_uid, joint_names)
# * parse options
verbose = options.get('verbose', True)
diagnosis = options.get('diagnosis', False)
avoid_collisions = options.get('avoid_collisions', True)
pos_step_size = options.get('max_step', 0.01)
planner_id = is_valid_option(options, 'planner_id', 'IterativeIK')
jump_threshold = is_valid_option(options, 'jump_threshold', {jt_name : math.pi/6 \
if jt_type in [Joint.REVOLUTE, Joint.CONTINUOUS] else 0.1 \
for jt_name, jt_type in zip(joint_names, joint_types)})
jump_threshold_from_joint = {
joint_from_name(robot_uid, jt_name): j_diff
for jt_name, j_diff in jump_threshold.items()
}
# * iterative IK options
pos_tolerance = is_valid_option(options, 'pos_tolerance', 1e-3)
ori_tolerance = is_valid_option(options, 'ori_tolerance', 1e-3 * np.pi)
# * ladder graph options
frame_variant_gen = is_valid_option(options, 'frame_variant_generator',
None)
ik_function = is_valid_option(options, 'ik_function', None)
# * convert to poses and do workspace linear interpolation
given_poses = [pose_from_frame(frame_WCF) for frame_WCF in frames_WCF]
ee_poses = []
for p1, p2 in zip(given_poses[:-1], given_poses[1:]):
c_interp_poses = list(
interpolate_poses(p1, p2, pos_step_size=pos_step_size))
ee_poses.extend(c_interp_poses)
# * build collision fn
attachments = values_as_list(self.client.pychoreo_attachments)
collision_fn = PyChoreoConfigurationCollisionChecker(
self.client)._get_collision_fn(robot, joint_names, options=options)
failure_reason = ''
with WorldSaver():
# set to start conf
if start_configuration is not None:
self.client.set_robot_configuration(robot, start_configuration)
if planner_id == 'IterativeIK':
selected_links = [
link_from_name(robot_uid, l)
for l in robot.get_link_names(group=group)
]
# with HideOutput():
# with redirect_stdout():
path = plan_cartesian_motion_from_links(
robot_uid,
selected_links,
tool_link,
ee_poses,
get_sub_conf=False,
pos_tolerance=pos_tolerance,
ori_tolerance=ori_tolerance)
if path is None:
failure_reason = 'IK plan is not found.'
# collision checking is not included in the default Cartesian planning
if path is not None and avoid_collisions:
for i, conf_val in enumerate(path):
pruned_conf_val = self._prune_configuration(
robot_uid, conf_val, joint_names)
for attachment in attachments:
attachment.assign()
if collision_fn(pruned_conf_val, diagnosis=diagnosis):
failure_reason = 'IK plan is found but collision violated.'
path = None
break
path[i] = pruned_conf_val
# TODO check joint threshold
elif planner_id == 'LadderGraph':
# get ik fn from client
# collision checking is turned off because collision checking is handled inside LadderGraph planner
ik_options = {'avoid_collisions': False, 'return_all': True}
sample_ik_fn = ik_function or self._get_sample_ik_fn(
robot, ik_options)
# convert ee_variant_fn
if frame_variant_gen is not None:
def sample_ee_fn(pose):
for v_frame in frame_variant_gen.generate_frame_variant(
frame_from_pose(pose)):
yield pose_from_frame(v_frame)
else:
sample_ee_fn = None
path, cost = plan_cartesian_motion_lg(robot_uid, ik_joints, ee_poses, sample_ik_fn, collision_fn, \
jump_threshold=jump_threshold_from_joint, sample_ee_fn=sample_ee_fn)
if verbose:
print('Ladder graph cost: {}'.format(cost))
else:
raise ValueError('Cartesian planner {} not implemented!',
planner_id)
if path is None:
if verbose:
cprint(
'No Cartesian motion found, due to {}!'.format(
failure_reason), 'red')
return None
else:
# TODO start_conf might have different number of joints with the given group?
start_traj_pt = None
if start_configuration is not None:
start_traj_pt = JointTrajectoryPoint(
values=start_configuration.values,
types=start_configuration.types)
start_traj_pt.joint_names = start_configuration.joint_names
jt_traj_pts = []
for i, conf in enumerate(path):
jt_traj_pt = JointTrajectoryPoint(values=conf,
types=joint_types)
jt_traj_pt.joint_names = joint_names
if start_traj_pt is not None:
# ! TrajectoryPoint doesn't copy over joint_names...
jtp = start_traj_pt.copy()
jtp.joint_names = start_traj_pt.joint_names
jtp.merge(jt_traj_pt)
jt_traj_pt = jtp
jt_traj_pt.time_from_start = Duration(i * 1, 0)
jt_traj_pts.append(jt_traj_pt)
if start_configuration is not None and not compare_configurations(
start_configuration,
jt_traj_pts[0],
jump_threshold,
verbose=verbose):
# if verbose:
# print()
# cprint('Joint jump from start conf, max diff {}'.format(start_configuration.max_difference(jt_traj_pts[0])), 'red')
# cprint('start conf {}'.format(['{:.4f}'.format(v) for v in start_configuration.values]), 'red')
# cprint('traj pt 0 {}'.format(['{:.4f}'.format(v) for v in jt_traj_pts[0].values]), 'red')
pass
# return None
# TODO check intermediate joint jump
trajectory = JointTrajectory(
trajectory_points=jt_traj_pts,
joint_names=jt_traj_pts[0].joint_names,
start_configuration=jt_traj_pts[0],
fraction=1.0)
return trajectory
def test_ctor_only_takes_integers():
d = Duration(2.5, 5e+8)
assert d.seconds == 2.5
def test_inequality():
assert Duration(10, 25) != Duration(10, 21)
def test_equality():
assert Duration(10, 25) == Duration(10, 25)
def test_seconds():
d = Duration(2, 5e+8)
assert d.seconds == 2.5
def test_to_data():
d = Duration(2, 5e+8)
data = d.to_data()
assert data['secs'] == 2
assert data['nsecs'] == 5e+8
def test_from_data():
d = Duration.from_data(dict(secs=2, nsecs=5e+8))
assert d.seconds == 2.5
def test_repr():
d1 = Duration(2, 5e+8)
d2 = eval(repr(d1))
assert d2.seconds == d1.seconds
def test_serialization(trj):
data = trj.to_data()
new_trj = JointTrajectory.from_data(data)
assert (new_trj.to_data() == data)
assert (new_trj.time_from_start == Duration(6, 0).seconds)
def test_trajectory_points(trj):
assert (trj.time_from_start == Duration(6, 0).seconds)
assert (len(trj.points) == 2)