def convert_constraints_to_rosmsg(constraints, header):
"""Convert COMPAS FAB constraints into ROS Messages."""
if not constraints:
return None
ros_constraints = Constraints()
for c in constraints:
if c.type == c.JOINT:
ros_constraints.joint_constraints.append(
JointConstraint.from_joint_constraint(c))
elif c.type == c.POSITION:
ros_constraints.position_constraints.append(
PositionConstraint.from_position_constraint(header, c))
elif c.type == c.ORIENTATION:
ros_constraints.orientation_constraints.append(
OrientationConstraint.from_orientation_constraint(header, c))
else:
raise NotImplementedError
return ros_constraints
def plan_motion_async(self,
callback,
errback,
goal_constraints,
base_link,
ee_link,
group,
joint_names,
joint_types,
start_configuration,
path_constraints=None,
trajectory_constraints=None,
planner_id='',
num_planning_attempts=8,
allowed_planning_time=2.,
max_velocity_scaling_factor=1.,
max_acceleration_scaling_factor=1.,
attached_collision_meshes=None,
workspace_parameters=None):
"""Asynchronous handler of MoveIt motion planner service."""
# http://docs.ros.org/jade/api/moveit_core/html/utils_8cpp_source.html
# TODO: if list of frames (goals) => receive multiple solutions?
header = Header(frame_id=base_link)
joint_state = JointState(header=header,
name=joint_names,
position=start_configuration.values)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if attached_collision_meshes:
for acm in attached_collision_meshes:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
# goal constraints
constraints = Constraints()
for c in goal_constraints:
if c.type == c.JOINT:
constraints.joint_constraints.append(
JointConstraint.from_joint_constraint(c))
elif c.type == c.POSITION:
constraints.position_constraints.append(
PositionConstraint.from_position_constraint(header, c))
elif c.type == c.ORIENTATION:
constraints.orientation_constraints.append(
OrientationConstraint.from_orientation_constraint(
header, c))
else:
raise NotImplementedError
goal_constraints = [constraints]
# path constraints
if path_constraints:
constraints = Constraints()
for c in path_constraints:
if c.type == c.JOINT:
constraints.joint_constraints.append(
JointConstraint.from_joint_constraint(c))
elif c.type == c.POSITION:
constraints.position_constraints.append(
PositionConstraint.from_position_constraint(header, c))
elif c.type == c.ORIENTATION:
constraints.orientation_constraints.append(
OrientationConstraint.from_orientation_constraint(
header, c))
else:
raise NotImplementedError
path_constraints = constraints
request = dict(
start_state=start_state,
goal_constraints=goal_constraints,
path_constraints=path_constraints,
trajectory_constraints=trajectory_constraints,
planner_id=planner_id,
group_name=group,
num_planning_attempts=num_planning_attempts,
allowed_planning_time=allowed_planning_time,
max_velocity_scaling_factor=max_velocity_scaling_factor,
max_acceleration_scaling_factor=max_velocity_scaling_factor)
# workspace_parameters=workspace_parameters
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)
self.GET_MOTION_PLAN(self, request, convert_to_trajectory, errback)