def move_arm_joint_goal(move_arm_client,
joint_names,
joint_positions,
allowed_contacts=[],
link_padding=[],
collision_operations=OrderedCollisionOperations()):
goal = MoveArmGoal()
goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
goal.motion_plan_request.planner_id = ''
goal.motion_plan_request.group_name = ARM_GROUP_NAME
goal.motion_plan_request.num_planning_attempts = 3
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint(j, p, 0.1, 0.1, 0.0) for (j,p) in zip(joint_names,joint_positions)]
goal.planning_scene_diff.allowed_contacts = allowed_contacts
goal.planning_scene_diff.link_padding = link_padding
goal.operations = collision_operations
move_arm_client.send_goal(goal)
finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0))
if not finished_within_time:
move_arm_client.cancel_goal()
rospy.logerr('timed out trying to achieve joint goal')
return False
else:
state = move_arm_client.get_state()
if state == GoalStatus.SUCCEEDED:
return True
else:
rospy.logerr('failed to achieve joint goal (returned status code %s)' % str(state))
return False
def move_arm_joint_goal(move_arm_client,
joint_names,
joint_positions,
allowed_contacts=[],
link_padding=[],
collision_operations=OrderedCollisionOperations()):
goal = MoveArmGoal()
goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
goal.motion_plan_request.planner_id = ''
goal.motion_plan_request.group_name = ARM_GROUP_NAME
goal.motion_plan_request.num_planning_attempts = 3
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint(j, p, 0.1, 0.1, 0.0)
for (j, p) in zip(joint_names, joint_positions)
]
goal.planning_scene_diff.allowed_contacts = allowed_contacts
goal.planning_scene_diff.link_padding = link_padding
goal.operations = collision_operations
move_arm_client.send_goal(goal)
finished_within_time = move_arm_client.wait_for_result(
rospy.Duration(200.0))
if not finished_within_time:
move_arm_client.cancel_goal()
rospy.logerr('timed out trying to achieve joint goal')
return False
else:
state = move_arm_client.get_state()
if state == GoalStatus.SUCCEEDED:
return True
else:
rospy.logerr(
'failed to achieve joint goal (returned status code %s)' %
str(state))
return False
def __move_arm(self, arm, position, orientation, frame_id, waiting_time,
ordered_collision_operations = None,
allowed_contacts = None):
goal = MoveArmGoal()
goal.motion_plan_request.group_name = arm
goal.motion_plan_request.num_planning_attempts = 2
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.allowed_planning_time = rospy.Duration(waiting_time/2.)
position_constraint = PositionConstraint()
position_constraint.header.frame_id = frame_id
if arm == "right_arm":
link_name = "r_wrist_roll_link"
client = self.move_right_arm_client
elif arm == "left_arm":
link_name = "l_wrist_roll_link"
client = self.move_left_arm_client
else:
rospy.logerr("Unknown arm: %s"%arm)
return False
position_constraint.link_name = link_name
position_constraint.position.x = position[0]
position_constraint.position.y = position[1]
position_constraint.position.z = position[2]
position_constraint.constraint_region_shape.type = position_constraint.constraint_region_shape.BOX
tolerance = 2 * 0.02
position_constraint.constraint_region_shape.dimensions = [tolerance, tolerance, tolerance]
position_constraint.constraint_region_orientation.x = 0.
position_constraint.constraint_region_orientation.y = 0.
position_constraint.constraint_region_orientation.z = 0.
position_constraint.constraint_region_orientation.w = 1.
position_constraint.weight = 1.0
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = frame_id
orientation_constraint.link_name = link_name
# orientation_constraint.type = dunno!
orientation_constraint.orientation.x = orientation[0]
orientation_constraint.orientation.y = orientation[1]
orientation_constraint.orientation.z = orientation[2]
orientation_constraint.orientation.w = orientation[3]
orientation_constraint.absolute_roll_tolerance = 0.04
orientation_constraint.absolute_pitch_tolerance = 0.04
orientation_constraint.absolute_yaw_tolerance = 0.04
orientation_constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.position_constraints.append(position_constraint)
goal.motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint)
if ordered_collision_operations is not None:
rospy.loginfo("Adding ordered collisions")
goal.operations = ordered_collision_operations
if allowed_contacts is not None:
rospy.loginfo("Adding allowed_contacts")
goal.planning_scene_diff.allowed_contacts = allowed_contacts
goal.disable_collision_monitoring = False
state = client.send_goal_and_wait(goal, rospy.Duration(waiting_time))
if state == actionlib.GoalStatus.SUCCEEDED:
return True
else:
return False
def __move_arm(self,
arm,
position,
orientation,
frame_id,
waiting_time,
ordered_collision_operations=None,
allowed_contacts=None):
goal = MoveArmGoal()
goal.motion_plan_request.group_name = arm
goal.motion_plan_request.num_planning_attempts = 2
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.allowed_planning_time = rospy.Duration(
waiting_time / 2.)
position_constraint = PositionConstraint()
position_constraint.header.frame_id = frame_id
if arm == "right_arm":
link_name = "r_wrist_roll_link"
client = self.move_right_arm_client
elif arm == "left_arm":
link_name = "l_wrist_roll_link"
client = self.move_left_arm_client
else:
rospy.logerr("Unknown arm: %s" % arm)
return False
position_constraint.link_name = link_name
position_constraint.position.x = position[0]
position_constraint.position.y = position[1]
position_constraint.position.z = position[2]
position_constraint.constraint_region_shape.type = position_constraint.constraint_region_shape.BOX
tolerance = 2 * 0.02
position_constraint.constraint_region_shape.dimensions = [
tolerance, tolerance, tolerance
]
position_constraint.constraint_region_orientation.x = 0.
position_constraint.constraint_region_orientation.y = 0.
position_constraint.constraint_region_orientation.z = 0.
position_constraint.constraint_region_orientation.w = 1.
position_constraint.weight = 1.0
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = frame_id
orientation_constraint.link_name = link_name
# orientation_constraint.type = dunno!
orientation_constraint.orientation.x = orientation[0]
orientation_constraint.orientation.y = orientation[1]
orientation_constraint.orientation.z = orientation[2]
orientation_constraint.orientation.w = orientation[3]
orientation_constraint.absolute_roll_tolerance = 0.04
orientation_constraint.absolute_pitch_tolerance = 0.04
orientation_constraint.absolute_yaw_tolerance = 0.04
orientation_constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.position_constraints.append(
position_constraint)
goal.motion_plan_request.goal_constraints.orientation_constraints.append(
orientation_constraint)
if ordered_collision_operations is not None:
rospy.loginfo("Adding ordered collisions")
goal.operations = ordered_collision_operations
if allowed_contacts is not None:
rospy.loginfo("Adding allowed_contacts")
goal.planning_scene_diff.allowed_contacts = allowed_contacts
goal.disable_collision_monitoring = False
state = client.send_goal_and_wait(goal, rospy.Duration(waiting_time))
if state == actionlib.GoalStatus.SUCCEEDED:
return True
else:
return False
