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 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 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)