def inverse_kinematics_async(self,
callback,
errback,
frame,
base_link,
group,
joint_names,
joint_positions,
avoid_collisions=True,
constraints=None,
attempts=8,
attached_collision_meshes=None):
"""Asynchronous handler of MoveIt IK service."""
header = Header(frame_id=base_link)
pose = Pose.from_frame(frame)
pose_stamped = PoseStamped(header, pose)
joint_state = JointState(name=joint_names,
position=joint_positions,
header=header)
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)
ik_request = PositionIKRequest(group_name=group,
robot_state=start_state,
constraints=constraints,
pose_stamped=pose_stamped,
avoid_collisions=avoid_collisions,
attempts=attempts)
self.GET_POSITION_IK(self, (ik_request, ), callback, errback)
def inverse_kinematics_async(self,
callback,
errback,
frame,
base_link,
group,
joint_names,
joint_positions,
avoid_collisions=True,
constraints=None,
attempts=8):
"""
"""
header = Header(frame_id=base_link)
pose = Pose.from_frame(frame)
pose_stamped = PoseStamped(header, pose)
joint_state = JointState(name=joint_names,
position=joint_positions,
header=header)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
ik_request = PositionIKRequest(group_name=group,
robot_state=start_state,
constraints=constraints,
pose_stamped=pose_stamped,
avoid_collisions=avoid_collisions,
attempts=attempts)
self.GET_POSITION_IK(self, (ik_request, ), callback, errback)
def compute_cartesian_path_async(self, callback, errback, frames,
base_link, ee_link, group, joint_names,
joint_positions, max_step,
avoid_collisions, path_constraints,
attached_collision_object):
"""
"""
header = Header(frame_id=base_link)
waypoints = [Pose.from_frame(frame) for frame in frames]
joint_state = JointState(header=header,
name=joint_names,
position=joint_positions)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if attached_collision_object:
start_state.attached_collision_objects = [
attached_collision_object
]
request = dict(header=header,
start_state=start_state,
group_name=group,
link_name=ee_link,
waypoints=waypoints,
max_step=float(max_step),
avoid_collisions=bool(avoid_collisions),
path_constraints=path_constraints)
self.GET_CARTESIAN_PATH(self, request, callback, errback)
def plan_cartesian_motion_async(self, callback, errback, robot, frames,
start_configuration, group, max_step,
jump_threshold, avoid_collisions,
path_constraints,
attached_collision_meshes):
"""Asynchronous handler of MoveIt cartesian motion planner service."""
base_link = robot.get_base_link_name(group)
ee_link = robot.get_end_effector_link_name(group)
joint_names = robot.get_configurable_joint_names(group)
header = Header(frame_id=base_link)
waypoints = [Pose.from_frame(frame) for frame in frames]
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)
path_constraints = self._convert_constraints_to_rosmsg(
path_constraints, header)
request = dict(header=header,
start_state=start_state,
group_name=group,
link_name=ee_link,
waypoints=waypoints,
max_step=float(max_step),
jump_threshold=float(jump_threshold),
avoid_collisions=bool(avoid_collisions),
path_constraints=path_constraints)
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 = robot.get_joint_types_by_names(
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 = robot.get_joint_types_by_names(
start_state.name)
trajectory.start_configuration = Configuration(
start_state.position, start_state_types)
callback(trajectory)
except Exception as e:
errback(e)
self.GET_CARTESIAN_PATH(self, request, convert_to_trajectory, errback)
def follow_configurations(self,
callback,
joint_names,
configurations,
timesteps,
timeout=60000):
if len(configurations) != len(timesteps):
raise ValueError(
"%d configurations must have %d timesteps, but %d given." %
(len(configurations), len(timesteps), len(timesteps)))
if not timeout:
timeout = timesteps[-1] * 1000 * 2
points = []
num_joints = len(configurations[0].values)
for config, time in zip(configurations, timesteps):
pt = RosMsgJointTrajectoryPoint(positions=config.values,
velocities=[0] * num_joints,
time_from_start=Time(secs=(time)))
points.append(pt)
joint_trajectory = RosMsgJointTrajectory(
Header(), joint_names, points) # specify header necessary?
return self.follow_joint_trajectory(joint_trajectory=joint_trajectory,
callback=callback,
timeout=timeout)
def test_posearray():
from compas.geometry import Frame
p = [Pose.from_frame(f) for f in [Frame.worldXY()]]
m = PoseArray(header=Header(), poses=p)
assert (
repr(m) ==
"PoseArray(header=Header(seq=0, stamp=Time(secs=0, nsecs=0), frame_id='/world'), poses=[Pose(position=Point(x=0.0, y=0.0, z=0.0), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0))])"
) # noqa E501
def motion_plan_goal_joint_positions_async(
self,
callback,
errback,
joint_positions_goal,
joint_names_goal,
tolerances,
base_link,
group,
joint_names,
joint_positions,
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_object=None):
"""
"""
# http://docs.ros.org/jade/api/moveit_core/html/utils_8cpp_source.html
header = Header(frame_id=base_link)
joint_state = JointState(header=header,
name=joint_names,
position=joint_positions)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if attached_collision_object:
start_state.attached_collision_objects = [
attached_collision_object
]
joint_constraints = []
for position, joint_name, tolerance in zip(joint_positions_goal,
joint_names_goal,
tolerances):
jcm = JointConstraint(joint_name, position, tolerance, tolerance)
joint_constraints.append(jcm)
# TODO: possibility to hand over more goal constraints
goal_constraints = [Constraints(joint_constraints=joint_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)
self.GET_MOTION_PLAN(self, request, callback, errback)
def plan_cartesian_motion_async(self, callback, errback,
frames_WCF, start_configuration=None, group=None, options=None):
"""Asynchronous handler of MoveIt cartesian motion planner service."""
joint_names = options['joint_names']
joint_types = options['joint_types']
joint_type_by_name = dict(zip(joint_names, joint_types))
header = Header(frame_id=options['base_link'])
waypoints = [Pose.from_frame(frame) for frame in frames_WCF]
joint_state = JointState(header=header,
name=start_configuration.joint_names,
position=start_configuration.joint_values)
start_state = RobotState(joint_state, MultiDOFJointState(header=header), is_diff=True)
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)
path_constraints = convert_constraints_to_rosmsg(options.get('path_constraints'), header)
request = dict(header=header,
start_state=start_state,
group_name=group,
link_name=options['link'],
waypoints=waypoints,
max_step=float(options.get('max_step', 0.01)),
jump_threshold=float(options.get('jump_threshold', 1.57)),
avoid_collisions=bool(options.get('avoid_collisions', True)),
path_constraints=path_constraints)
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)
trajectory.attached_collision_meshes = list(itertools.chain(*[
aco.to_attached_collision_meshes()
for aco in response.start_state.attached_collision_objects]))
callback(trajectory)
except Exception as e:
errback(e)
self.GET_CARTESIAN_PATH(self.ros_client, request, convert_to_trajectory, errback)
def forward_kinematics_async(self, callback, errback, joint_positions,
base_link, group, joint_names, ee_link):
"""Asynchronous handler of MoveIt FK service."""
header = Header(frame_id=base_link)
fk_link_names = [ee_link]
joint_state = JointState(name=joint_names,
position=joint_positions,
header=header)
robot_state = RobotState(joint_state,
MultiDOFJointState(header=header))
self.GET_POSITION_FK(self, (header, fk_link_names, robot_state),
callback, errback)
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 forward_kinematics_async(self, callback, errback, robot, configuration,
group, ee_link):
"""Asynchronous handler of MoveIt FK service."""
base_link = robot.model.root.name
header = Header(frame_id=base_link)
fk_link_names = [ee_link]
joint_state = JointState(name=configuration.joint_names,
position=configuration.values,
header=header)
robot_state = RobotState(joint_state,
MultiDOFJointState(header=header))
def convert_to_frame(response):
callback(response.pose_stamped[0].pose.frame)
self.GET_POSITION_FK(self, (header, fk_link_names, robot_state),
convert_to_frame, errback)
def forward_kinematics_async(self, callback, errback,
configuration, options):
"""Asynchronous handler of MoveIt FK service."""
base_link = options['base_link']
fk_link_names = [options['link']]
header = Header(frame_id=base_link)
joint_state = JointState(
name=configuration.joint_names, position=configuration.joint_values, header=header)
robot_state = RobotState(
joint_state, MultiDOFJointState(header=header))
def convert_to_frame(response):
callback(response.pose_stamped[0].pose.frame)
self.GET_POSITION_FK(self.ros_client, (header, fk_link_names,
robot_state), convert_to_frame, errback)
def inverse_kinematics_async(self,
callback,
errback,
robot,
frame,
group,
start_configuration,
avoid_collisions=True,
constraints=None,
attempts=8,
attached_collision_meshes=None):
"""Asynchronous handler of MoveIt IK service."""
base_link = robot.model.root.name
header = Header(frame_id=base_link)
pose = Pose.from_frame(frame)
pose_stamped = PoseStamped(header, pose)
joint_state = JointState(name=start_configuration.joint_names,
position=start_configuration.values,
header=header)
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)
constraints = self._convert_constraints_to_rosmsg(constraints, header)
ik_request = PositionIKRequest(group_name=group,
robot_state=start_state,
constraints=constraints,
pose_stamped=pose_stamped,
avoid_collisions=avoid_collisions,
attempts=attempts)
def convert_to_positions(response):
callback((response.solution.joint_state.position,
response.solution.joint_state.name))
self.GET_POSITION_IK(self, (ik_request, ), convert_to_positions,
errback)
def build_collision_object(self, frame_id, id_name, compas_mesh,
operation):
co = CollisionObject(header=Header(frame_id=frame_id), id=id_name)
if compas_mesh:
# ROS mesh message requires triangles
mesh_quads_to_triangles(compas_mesh)
mesh = Mesh.from_mesh(compas_mesh)
co.meshes = [mesh]
co.mesh_poses = [Pose()]
if operation == 0:
co.operation = CollisionObject.ADD
elif operation == 1:
co.operation = CollisionObject.REMOVE
elif operation == 2:
co.operation = CollisionObject.APPEND
else:
raise ValueError("Operation unknown")
return co
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.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)
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))
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 test_nested_repr():
t = Time(80, 20)
h = Header(seq=10, stamp=t, frame_id='/wow')
assert repr(h) == "Header(seq=10, stamp=Time(secs=80, nsecs=20), frame_id='/wow')"
def plan_motion_async(self,
callback,
errback,
robot,
goal_constraints,
start_configuration,
group,
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?
base_link = robot.model.root.name # use world coords
header = Header(frame_id=base_link)
joint_state = JointState(header=header,
name=start_configuration.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)
# convert constraints
goal_constraints = [
self._convert_constraints_to_rosmsg(goal_constraints, header)
]
path_constraints = self._convert_constraints_to_rosmsg(
path_constraints, header)
if trajectory_constraints is not None:
trajectory_constraints = TrajectoryConstraints(
constraints=self._convert_constraints_to_rosmsg(
path_constraints, header))
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.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, start_state.name)
callback(trajectory)
self.GET_MOTION_PLAN(self, request, convert_to_trajectory, errback)
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)
def plan_motion_async(self,
callback,
errback,
goal_constraints,
start_configuration=None,
group=None,
options=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?
joint_names = options['joint_names']
joint_types = options['joint_types']
joint_type_by_name = dict(
zip(joint_names, joint_types)
) # !!! should this go somewhere else? does it already exist somewhere else?
header = Header(frame_id=options['base_link'])
joint_state = JointState(header=header,
name=start_configuration.joint_names,
position=start_configuration.joint_values)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header),
is_diff=True)
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)
# convert constraints
goal_constraints = [
convert_constraints_to_rosmsg(goal_constraints, header)
]
path_constraints = convert_constraints_to_rosmsg(
options.get('path_constraints'), header)
trajectory_constraints = options.get('trajectory_constraints')
if trajectory_constraints is not None:
trajectory_constraints = TrajectoryConstraints(
constraints=convert_constraints_to_rosmsg(
options['trajectory_constraints'], header))
request = dict(
start_state=start_state,
goal_constraints=goal_constraints,
path_constraints=path_constraints,
trajectory_constraints=trajectory_constraints,
planner_id=options.get('planner_id', 'RRTConnectkConfigDefault'),
group_name=group,
num_planning_attempts=options.get('num_planning_attempts', 1),
allowed_planning_time=options.get('allowed_planning_time', 2.),
max_velocity_scaling_factor=options.get(
'max_velocity_scaling_factor', 1.),
max_acceleration_scaling_factor=options.get(
'max_acceleration_scaling_factor', 1.))
# workspace_parameters=options.get('workspace_parameters')
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)
self.GET_MOTION_PLAN(self.ros_client, request, convert_to_trajectory,
errback)
def plan_motion_async(self,
callback,
errback,
goal_constraints,
start_configuration=None,
group=None,
options=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?
joints = options['joints']
header = Header(frame_id=options['base_link'])
joint_state = JointState(header=header,
name=start_configuration.joint_names,
position=start_configuration.joint_values)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header),
is_diff=True)
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)
# convert constraints
goal_constraints = [
convert_constraints_to_rosmsg(goal_constraints, header)
]
path_constraints = convert_constraints_to_rosmsg(
options.get('path_constraints'), header)
trajectory_constraints = options.get('trajectory_constraints')
if trajectory_constraints is not None:
trajectory_constraints = TrajectoryConstraints(
constraints=convert_constraints_to_rosmsg(
options['trajectory_constraints'], header))
request = dict(
start_state=start_state,
goal_constraints=goal_constraints,
path_constraints=path_constraints,
trajectory_constraints=trajectory_constraints,
planner_id=options.get('planner_id', 'RRTConnect'),
group_name=group,
num_planning_attempts=options.get('num_planning_attempts', 1),
allowed_planning_time=options.get('allowed_planning_time', 2.),
max_velocity_scaling_factor=options.get(
'max_velocity_scaling_factor', 1.),
max_acceleration_scaling_factor=options.get(
'max_acceleration_scaling_factor', 1.))
# workspace_parameters=options.get('workspace_parameters')
def response_handler(response):
try:
trajectory = convert_trajectory(joints, response.trajectory,
response.trajectory_start, 1.,
response.planning_time,
response)
callback(trajectory)
except Exception as e:
errback(e)
self.GET_MOTION_PLAN(self.ros_client, request, response_handler,
errback)
def motion_plan_goal_frame_async(self,
callback,
errback,
frame,
base_link,
ee_link,
group,
joint_names,
joint_positions,
tolerance_position,
tolerance_angle,
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_object=None):
"""
"""
# 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=joint_positions)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if attached_collision_object:
start_state.attached_collision_objects = [
attached_collision_object
]
pose = Pose.from_frame(frame)
pcm = PositionConstraint(header=header, link_name=ee_link)
pcm.target_point_offset.x = 0.
pcm.target_point_offset.y = 0.
pcm.target_point_offset.z = 0.
bv = SolidPrimitive(type=SolidPrimitive.SPHERE,
dimensions=[tolerance_position])
pcm.constraint_region.primitives = [bv]
pcm.constraint_region.primitive_poses = [
Pose(pose.position, Quaternion(0, 0, 0, 1))
]
ocm = OrientationConstraint(header=header, link_name=ee_link)
ocm.orientation = Quaternion.from_frame(frame)
ocm.absolute_x_axis_tolerance = tolerance_angle
ocm.absolute_y_axis_tolerance = tolerance_angle
ocm.absolute_z_axis_tolerance = tolerance_angle
# TODO: possibility to hand over more goal constraints
goal_constraints = [
Constraints(position_constraints=[pcm],
orientation_constraints=[ocm])
]
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)
self.GET_MOTION_PLAN(self, request, callback, errback)
