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 move_to_state(self, joint_states):
if self.verbose:
print 'Moving to next state'
mpr = MotionPlanRequest()
con = Constraints()
for name, val in joint_states.items():
if name not in self.joints_to_exclude:
jc = JointConstraint()
jc.joint_name = name
jc.position = val
jc.tolerance_above = self.joint_angle_tolerance
jc.tolerance_below = self.joint_angle_tolerance
jc.weight = 1.0
con.joint_constraints.append(jc)
mpr.goal_constraints = [con]
mpr.group_name = self.planning_group_name
mpr.allowed_planning_time = 3.0 # [s]
# print mpr
try:
req = rospy.ServiceProxy('/plan_kinematic_path', GetMotionPlan)
res = req(mpr)
traj = res.motion_plan_response.trajectory
except rospy.ServiceException, e:
print "Service call failed: %s" % e
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
