def create_basic_mp_position_request(
planning_frame,
link_name,
target_point_offset,
planner_id):
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = move_group
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = 5.0
motion_plan_request.workspace_parameters = WorkspaceParameters()
motion_plan_request.max_velocity_scaling_factor = 0.5
motion_plan_request.max_acceleration_scaling_factor = 0.5
motion_plan_request.planner_id = planner_id
position_constraints = []
position_constraint = PositionConstraint()
header = std_msgs.msg.Header()
header.frame_id = planning_frame
position_constraint.header = header
position_constraint.link_name = link_name
position_constraint.target_point_offset = target_point_offset
position_constraints = [position_constraint]
constraint = Constraints()
constraint.position_constraints = position_constraints
constraints = [constraint]
motion_plan_request.goal_constraints = constraints
return motion_plan_request
def _cmd_to_request(self, robot):
"""Transforms the gripper command to a MotionPlanRequest."""
req = MotionPlanRequest()
# Set general info
req.planner_id = "PTP"
req.group_name = self._planning_group
req.max_velocity_scaling_factor = self._vel_scale * robot.global_motion_factor
req.max_acceleration_scaling_factor = self._acc_scale * robot.global_motion_factor
req.allowed_planning_time = 1.0
# Set an empty diff as start_state => the current state is used by the planner
req.start_state.is_diff = True
# create goal constraints
goal_constraints = Constraints()
if isinstance(self._goal, (float, int, long)):
joint_names = robot._robot_commander.get_group(self._planning_group).get_active_joints()
if len(joint_names) != 1:
raise IndexError("PG70 should have only one joint. But group " + req.group_name +
" contains " + str(len(joint_names)) + " joints.")
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_names[0]
joint_constraint.position = float(self._goal)
joint_constraint.weight = 1
goal_constraints.joint_constraints.append(joint_constraint)
else:
raise NotImplementedError("Unknown type of goal is given.")
req.goal_constraints.append(goal_constraints)
return req
def _cmd_to_request(self, robot):
"""Transforms the given command to a MotionPlanRequest."""
req = MotionPlanRequest()
# Set general info
req.planner_id = self._planner_id
req.group_name = self._planning_group
req.max_velocity_scaling_factor = self._vel_scale
req.max_acceleration_scaling_factor = self._acc_scale
req.allowed_planning_time = 1.0
# Set an empty diff as start_state => the current state is used by the planner
req.start_state.is_diff = True
# Set goal constraint
if self._goal is None:
raise NameError("Goal is not given.")
goal_constraints = Constraints()
# goal as Pose in Cartesian space
if isinstance(self._goal, Pose):
goal_pose = self._get_goal_pose(robot)
robot_reference_frame = robot._robot_commander.get_planning_frame()
goal_constraints.orientation_constraints.append(
_to_ori_constraint(goal_pose, robot_reference_frame,
self._target_link))
goal_constraints.position_constraints.append(
_to_pose_constraint(goal_pose, robot_reference_frame,
self._target_link))
# goal as list of int or float in joint space
elif isinstance(self._goal, list):
joint_names = robot._robot_commander.get_group(
self._planning_group).get_active_joints()
joint_values = self._get_joint_pose(robot)
if len(joint_names) != len(joint_values):
raise IndexError(
"Given joint goal does not match the planning group " +
req.group_name + ".")
for joint_name, joint_value in zip(joint_names, joint_values):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_name
joint_constraint.position = joint_value
joint_constraint.weight = 1
goal_constraints.joint_constraints.append(joint_constraint)
else:
raise NotImplementedError("Unknown type of goal is given.")
req.goal_constraints.append(goal_constraints)
return req
def create_basic_mp_joint_request(joint_names, joint_values, planner_id):
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = move_group
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = 5.0
motion_plan_request.workspace_parameters = WorkspaceParameters()
motion_plan_request.max_velocity_scaling_factor = 0.5
motion_plan_request.max_acceleration_scaling_factor = 0.5
motion_plan_request.planner_id = planner_id
joint_constraints = []
for i in range(len(joint_names)):
joint_con = JointConstraint()
joint_con.joint_name = joint_names[i]
joint_con.position = joint_values[i]
joint_con.tolerance_above = 0.0001
joint_con.tolerance_below = 0.0001
joint_con.weight = 0.0001
joint_constraints.append(joint_con)
constraint = Constraints()
constraint.joint_constraints = joint_constraints
constraints = [constraint]
motion_plan_request.goal_constraints = constraints
return motion_plan_request
