def pose_constraint_to_position_orientation_constraints(pose_constraint):
position_constraint = PositionConstraint()
orientation_constraint = OrientationConstraint()
position_constraint.header = pose_constraint.header
position_constraint.link_name = pose_constraint.link_name
position_constraint.position = pose_constraint.pose.position
position_constraint.constraint_region_shape.type = 0
position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.x)
position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.y)
position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.z)
position_constraint.constraint_region_orientation.x = 0.0
position_constraint.constraint_region_orientation.y = 0.0
position_constraint.constraint_region_orientation.z = 0.0
position_constraint.constraint_region_orientation.w = 1.0
position_constraint.weight = 1.0
orientation_constraint.header = pose_constraint.header
orientation_constraint.link_name = pose_constraint.link_name
orientation_constraint.orientation = pose_constraint.pose.orientation
orientation_constraint.type = pose_constraint.orientation_constraint_type
orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance
orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance
orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance
orientation_constraint.weight = 1.0
return (position_constraint, orientation_constraint)
def get_interpolated_ik_motion_plan(self,
start_pose,
goal_pose,
start_angles,
joint_names,
pos_spacing=0.01,
rot_spacing=0.1,
consistent_angle=math.pi / 9,
collision_aware=True,
collision_check_resolution=1,
steps_before_abort=-1,
num_steps=0,
ordered_collision_operations=None,
frame='base_footprint',
start_from_end=0,
max_joint_vels=[1.5] * 7,
max_joint_accs=[8.0] * 7):
res = self.interpolated_ik_params_srv(num_steps, consistent_angle,
collision_check_resolution,
steps_before_abort, pos_spacing,
rot_spacing, collision_aware,
start_from_end, max_joint_vels,
max_joint_accs)
req = GetMotionPlanRequest()
req.motion_plan_request.start_state.joint_state.name = joint_names
req.motion_plan_request.start_state.joint_state.position = start_angles
req.motion_plan_request.start_state.multi_dof_joint_state.poses = [
start_pose.pose
]
req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [
GRIPPER_LINK_FRAME
]
req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [
start_pose.header.frame_id
]
pos_constraint = PositionConstraint()
pos_constraint.position = goal_pose.pose.position
pos_constraint.header.frame_id = goal_pose.header.frame_id
req.motion_plan_request.goal_constraints.position_constraints = [
pos_constraint,
]
orient_constraint = OrientationConstraint()
orient_constraint.orientation = goal_pose.pose.orientation
orient_constraint.header.frame_id = goal_pose.header.frame_id
req.motion_plan_request.goal_constraints.orientation_constraints = [
orient_constraint,
]
#req.motion_plan_request.link_padding = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
#req.motion_plan_request.link_padding.extend([LinkPadding(l,0.0) for l in ARM_LINKS])
#if ordered_collision_operations is not None:
# req.motion_plan_request.ordered_collision_operations = ordered_collision_operations
res = self.interpolated_ik_srv(req)
return res
def poseConstraintToPositionOrientationConstraints(pose_constraint):
position_constraint = PositionConstraint()
orientation_constraint = OrientationConstraint()
position_constraint.header = pose_constraint.header
position_constraint.link_name = pose_constraint.link_name
position_constraint.position = pose_constraint.pose.position
position_constraint.constraint_region_shape.type = 0
position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.x)
position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.y)
position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.z)
position_constraint.constraint_region_orientation.x = 0.0
position_constraint.constraint_region_orientation.y = 0.0
position_constraint.constraint_region_orientation.z = 0.0
position_constraint.constraint_region_orientation.w = 1.0
position_constraint.weight = 1.0
orientation_constraint.header = pose_constraint.header
orientation_constraint.link_name = pose_constraint.link_name
orientation_constraint.orientation = pose_constraint.pose.orientation
orientation_constraint.type = pose_constraint.orientation_constraint_type
orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance
orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance
orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance
orientation_constraint.weight = 1.0
return (position_constraint, orientation_constraint)
def get_interpolated_ik_motion_plan(
self,
start_pose,
goal_pose,
start_angles,
joint_names,
pos_spacing=0.01,
rot_spacing=0.1,
consistent_angle=math.pi / 9,
collision_aware=True,
collision_check_resolution=1,
steps_before_abort=-1,
num_steps=0,
ordered_collision_operations=None,
frame="base_footprint",
start_from_end=0,
max_joint_vels=[1.5] * 7,
max_joint_accs=[8.0] * 7,
):
res = self.interpolated_ik_params_srv(
num_steps,
consistent_angle,
collision_check_resolution,
steps_before_abort,
pos_spacing,
rot_spacing,
collision_aware,
start_from_end,
max_joint_vels,
max_joint_accs,
)
req = GetMotionPlanRequest()
req.motion_plan_request.start_state.joint_state.name = joint_names
req.motion_plan_request.start_state.joint_state.position = start_angles
req.motion_plan_request.start_state.multi_dof_joint_state.poses = [start_pose.pose]
req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [GRIPPER_LINK_FRAME]
req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [start_pose.header.frame_id]
pos_constraint = PositionConstraint()
pos_constraint.position = goal_pose.pose.position
pos_constraint.header.frame_id = goal_pose.header.frame_id
req.motion_plan_request.goal_constraints.position_constraints = [pos_constraint]
orient_constraint = OrientationConstraint()
orient_constraint.orientation = goal_pose.pose.orientation
orient_constraint.header.frame_id = goal_pose.header.frame_id
req.motion_plan_request.goal_constraints.orientation_constraints = [orient_constraint]
# req.motion_plan_request.link_padding = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
# req.motion_plan_request.link_padding.extend([LinkPadding(l,0.0) for l in ARM_LINKS])
# if ordered_collision_operations is not None:
# req.motion_plan_request.ordered_collision_operations = ordered_collision_operations
res = self.interpolated_ik_srv(req)
return res
def test_move_arm(position, orientation, frame):
rospy.loginfo("Moving the arm to x: %f y: %f z: %f r: %f p: %f y: %f", position[0], position[1], position[2], orientation[0], orientation[1], orientation[2]);
client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
client.wait_for_server()
goal = MoveArmGoal()
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 3
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.allowed_planning_time = rospy.Duration(15.)
position_constraint = PositionConstraint()
position_constraint.header.frame_id = frame
position_constraint.link_name = "r_wrist_roll_link"
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 = 0.04
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
orientation_constraint.link_name = "r_wrist_roll_link"
orientation_constraint.orientation = quaternion_to_msg(tf.transformations.quaternion_from_euler(orientation[0], orientation[1], orientation[2]))
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)
goal.disable_collision_monitoring = True
rospy.loginfo("Calling the move arm client")
state = client.send_goal_and_wait(goal, rospy.Duration(120.))
if state == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo("Succeeded")
else:
rospy.loginfo(state)
def processFeedback(feedback):
#s = "Feedback from marker '" + feedback.marker_name
#if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# rospy.loginfo( s + ": button click" + mp + "." )
if feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
if (feedback.menu_entry_id == 1):
p = feedback.pose
print p
goalA = arm_navigation_msgs.msg.MoveArmGoal()
goalA.motion_plan_request.group_name = "right";
goalA.motion_plan_request.num_planning_attempts = 1;
goalA.motion_plan_request.planner_id = "";
goalA.planner_service_name = "ompl_planning/plan_kinematic_path";
goalA.motion_plan_request.allowed_planning_time = rospy.Duration(5.0);
desired_pose = PositionConstraint()
desired_pose.header.frame_id = "/world";
desired_pose.link_name = "right_arm_7_link";
desired_pose.position = p.position
desired_pose.constraint_region_shape.type = Shape.BOX
desired_pose.constraint_region_shape.dimensions = [0.02, 0.02, 0.02]
desired_pose.constraint_region_orientation.w = 1.0
goalA.motion_plan_request.goal_constraints.position_constraints.append(desired_pose)
oc = OrientationConstraint()
oc.header.stamp = rospy.Time.now()
oc.header.frame_id = "/world";
oc.link_name = "right_arm_7_link";
oc.orientation = p.orientation
oc.absolute_roll_tolerance = 0.04
oc.absolute_pitch_tolerance = 0.04
oc.absolute_yaw_tolerance = 0.04
oc.weight = 1.
goalA.motion_plan_request.goal_constraints.orientation_constraints.append(oc)
client.send_goal(goalA)
req = GetMotionPlanRequest()
req.motion_plan_request.start_state.joint_state.name = joint_names
req.motion_plan_request.start_state.joint_state.position = start_angles
req.motion_plan_request.start_state.multi_dof_joint_state.pose = start_pose.pose
req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_id = 'L7_wrist_yaw_link'
req.motion_plan_request.start_state.multi_dof_joint_state.frame_id = start_pose.header.frame_id
pos_constraint = PositionConstraint()
pos_constraint.position = goal_pose.pose.position
pos_constraint.header.frame_id = goal_pose.header.frame_id
req.motion_plan_request.goal_constraints.position_constraints = [
pos_constraint,
]
orient_constraint = OrientationConstraint()
orient_constraint.orientation = goal_pose.pose.orientation
orient_constraint.header.frame_id = goal_pose.header.frame_id
req.motion_plan_request.goal_constraints.orientation_constraints = [
orient_constraint,
]
if ordered_collision_operations != None:
req.motion_plan_request.ordered_collision_operations = ordered_collision_operations
try:
serv = rospy.ServiceProxy("l_interpolated_ik_motion_plan",
GetMotionPlan)
res = serv(req)
except rospy.ServiceException as e:
print "error when calling l_interpolated_ik_motion_plan: %s" % e
return 0
def move_arm_to(self, goal_in):
(reachable, ik_goal, duration) = self.full_ik_check(goal_in)
rospy.sleep(duration)
rospy.loginfo("Composing move_" + self.arm + "_arm goal")
goal_out = MoveArmGoal()
goal_out.motion_plan_request.group_name = self.arm + "_arm"
goal_out.motion_plan_request.num_planning_attempts = 10
goal_out.motion_plan_request.planner_id = ""
goal_out.planner_service_name = "ompl_planning/plan_kinematic_path"
goal_out.motion_plan_request.allowed_planning_time = rospy.Duration(
1.0)
pos = PositionConstraint()
pos.header.frame_id = goal_in.header.frame_id
pos.link_name = self.arm[0] + "_wrist_roll_link"
pos.position = goal_in.pose.position
pos.constraint_region_shape.type = 0
pos.constraint_region_shape.dimensions = [0.01]
pos.constraint_region_orientation = Quaternion(0, 0, 0, 1)
pos.weight = 1
goal_out.motion_plan_request.goal_constraints.position_constraints.append(
pos)
ort = OrientationConstraint()
ort.header.frame_id = goal_in.header.frame_id
ort.link_name = self.arm[0] + "_wrist_roll_link"
ort.orientation = goal_in.pose.orientation
ort.absolute_roll_tolerance = 0.02
ort.absolute_pitch_tolerance = 0.02
ort.absolute_yaw_tolerance = 0.02
ort.weight = 0.5
goal_out.motion_plan_request.goal_constraints.orientation_constraints.append(
ort)
goal_out.accept_partial_plans = True
goal_out.accept_invalid_goals = True
goal_out.disable_collision_monitoring = True
rospy.loginfo("Sending composed move_" + self.arm + "_arm goal")
finished_within_time = False
self.move_arm_client.send_goal(goal_out)
finished_within_time = self.move_arm_client.wait_for_result(
rospy.Duration(60))
if not (finished_within_time):
self.move_arm_client.cancel_goal()
self.log_out.publish(data="Timed out achieving " + self.arm +
" arm goal pose")
rospy.loginfo("Timed out achieving right arm goal pose")
return False
else:
state = self.move_arm_client.get_state()
result = self.move_arm_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Action Finished: %s" % state)
self.log_out.publish(
data="Move " + self.arm.capitalize() +
" Arm with Motion Planning: %s" %
self.move_arm_error_dict[result.error_code.val])
return True
else:
if result.error_code.val == 1:
rospy.loginfo("Move_" + self.arm + "_arm_action failed: \
Unable to plan a path to goal")
self.log_out.publish(data="Move " + self.arm.capitalize() +
" Arm with Motion Planning Failed: \
Unable to plan a path to the goal")
elif result.error_code.val == -31:
(torso_succeeded,
pos) = self.check_torso(self.form_ik_request(goal_in))
if torso_succeeded:
rospy.loginfo("IK Failed in Current Position. \
Adjusting Height and Retrying")
self.log_out.publish(data="IK Failed in Current \
Position. Adjusting \
Height and Retrying")
self.move_arm_to(pos)
else:
rospy.loginfo("Move_" + self.arm +
"_arm action failed: %s" % state)
rospy.loginfo("Failure Result: %s" % result)
self.log_out.publish(
data="Move " + self.arm.capitalize() +
" Arm with Motion Planning \
and Torso Check Failed: %s"
% self.move_arm_error_dict[result.error_code.val])
else:
rospy.loginfo("Move_" + self.arm +
"_arm action failed: %s" % state)
rospy.loginfo("Failure Result: %s" % result)
self.log_out.publish(
data="Move " + self.arm.capitalize() +
" Arm with Motion Planning \
Failed: %s" %
self.move_arm_error_dict[result.error_code.val])
return False
print "error when calling r_interpolated_ik_motion_plan_set_params: %s"%e
return 0
req = GetMotionPlanRequest()
req.motion_plan_request.start_state.joint_state.name = joint_names
req.motion_plan_request.start_state.joint_state.position = start_angles
req.motion_plan_request.start_state.multi_dof_joint_state.poses.append(start_pose.pose)
req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids.append('r_wrist_roll_link')
req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids.append(start_pose.header.frame_id)
pos_constraint = PositionConstraint()
pos_constraint.position = goal_pose.pose.position
pos_constraint.header.frame_id = goal_pose.header.frame_id
req.motion_plan_request.goal_constraints.position_constraints = [pos_constraint,]
orient_constraint = OrientationConstraint()
orient_constraint.orientation = goal_pose.pose.orientation
orient_constraint.header.frame_id = goal_pose.header.frame_id
req.motion_plan_request.goal_constraints.orientation_constraints = [orient_constraint,]
#if ordered_collision_operations != None:
# req.motion_plan_request.ordered_collision_operations = ordered_collision_operations
try:
serv = rospy.ServiceProxy("r_interpolated_ik_motion_plan", GetMotionPlan)
res = serv(req)
except rospy.ServiceException, e:
print "error when calling r_interpolated_ik_motion_plan: %s"%e
return 0
error_code_dict = {ArmNavigationErrorCodes.SUCCESS:0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED:1, \
ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED:2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED:3, \
def move_arm_to(self, goal_in):
(reachable, ik_goal, duration) = self.full_ik_check(goal_in)
rospy.sleep(duration)
rospy.loginfo("Composing move_"+self.arm+"_arm goal")
goal_out = MoveArmGoal()
goal_out.motion_plan_request.group_name = self.arm+"_arm"
goal_out.motion_plan_request.num_planning_attempts = 10
goal_out.motion_plan_request.planner_id = ""
goal_out.planner_service_name = "ompl_planning/plan_kinematic_path"
goal_out.motion_plan_request.allowed_planning_time = rospy.Duration(1.0)
pos = PositionConstraint()
pos.header.frame_id = goal_in.header.frame_id
pos.link_name = self.arm[0]+"_wrist_roll_link"
pos.position = goal_in.pose.position
pos.constraint_region_shape.type = 0
pos.constraint_region_shape.dimensions=[0.01]
pos.constraint_region_orientation = Quaternion(0,0,0,1)
pos.weight = 1
goal_out.motion_plan_request.goal_constraints.position_constraints.append(pos)
ort = OrientationConstraint()
ort.header.frame_id=goal_in.header.frame_id
ort.link_name = self.arm[0]+"_wrist_roll_link"
ort.orientation = goal_in.pose.orientation
ort.absolute_roll_tolerance = 0.02
ort.absolute_pitch_tolerance = 0.02
ort.absolute_yaw_tolerance = 0.02
ort.weight = 0.5
goal_out.motion_plan_request.goal_constraints.orientation_constraints.append(ort)
goal_out.accept_partial_plans = True
goal_out.accept_invalid_goals = True
goal_out.disable_collision_monitoring = True
rospy.loginfo("Sending composed move_"+self.arm+"_arm goal")
finished_within_time = False
self.move_arm_client.send_goal(goal_out)
finished_within_time = self.move_arm_client.wait_for_result(
rospy.Duration(60))
if not (finished_within_time):
self.move_arm_client.cancel_goal()
self.log_out.publish(data="Timed out achieving "+self.arm+
" arm goal pose")
rospy.loginfo("Timed out achieving right arm goal pose")
return False
else:
state = self.move_arm_client.get_state()
result = self.move_arm_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Action Finished: %s" %state)
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning: %s"
%self.move_arm_error_dict[
result.error_code.val])
return True
else:
if result.error_code.val == 1:
rospy.loginfo("Move_"+self.arm+"_arm_action failed: \
Unable to plan a path to goal")
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning Failed: \
Unable to plan a path to the goal")
elif result.error_code.val == -31:
(torso_succeeded, pos) = self.check_torso(
self.form_ik_request(goal_in))
if torso_succeeded:
rospy.loginfo("IK Failed in Current Position. \
Adjusting Height and Retrying")
self.log_out.publish(data="IK Failed in Current \
Position. Adjusting \
Height and Retrying")
self.move_arm_to(pos)
else:
rospy.loginfo("Move_"+self.arm+"_arm action failed: %s"
%state)
rospy.loginfo("Failure Result: %s" %result)
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning \
and Torso Check Failed: %s"
%self.move_arm_error_dict[
result.error_code.val])
else:
rospy.loginfo("Move_"+self.arm+"_arm action failed: %s" %state)
rospy.loginfo("Failure Result: %s" %result)
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning \
Failed: %s"
%self.move_arm_error_dict[
result.error_code.val])
return False