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 testMotionExecutionBuffer(self):
global padd_name
global extra_buffer
#too much trouble to read for now
allow_padd = .05 #rospy.get_param(padd_name)
joint_names = [
'%s_%s' % ('r', j) for j in [
'shoulder_pan_joint', 'shoulder_lift_joint',
'upper_arm_roll_joint', 'elbow_flex_joint',
'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'
]
]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "chomp_planner_longrange/plan_path"
goal.disable_collision_monitoring = True
r = rospy.Rate(10)
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
for z in range(6):
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[
i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[
i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_below = 0.08
if (z % 2 == 0):
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = -1.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
else:
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = .22
goal.motion_plan_request.goal_constraints.joint_constraints[
1].position = .51
goal.motion_plan_request.goal_constraints.joint_constraints[
2].position = .05
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.41
goal.motion_plan_request.goal_constraints.joint_constraints[
4].position = -.06
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -.1
goal.motion_plan_request.goal_constraints.joint_constraints[
6].position = 0.0
for x in range(3):
self.move_arm_action_client.send_goal(goal)
while True:
cur_state = self.move_arm_action_client.get_state()
if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
and cur_state !=
actionlib_msgs.msg.GoalStatus.PENDING):
break
r.sleep()
end_state = self.move_arm_action_client.get_state()
if (end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED):
break
final_state = self.move_arm_action_client.get_state()
self.assertEqual(final_state,
actionlib_msgs.msg.GoalStatus.SUCCEEDED)
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
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
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 testMotionExecutionBuffer(self):
global padd_name
global extra_buffer
#too much trouble to read for now
allow_padd = .05#rospy.get_param(padd_name)
joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "chomp_planner_longrange/plan_path"
goal.disable_collision_monitoring = True;
r = rospy.Rate(10)
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
for z in range(6):
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
if(z%2==0):
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -1.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
else:
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = .22
goal.motion_plan_request.goal_constraints.joint_constraints[1].position = .51
goal.motion_plan_request.goal_constraints.joint_constraints[2].position = .05
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.41
goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -.06
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -.1
goal.motion_plan_request.goal_constraints.joint_constraints[6].position = 0.0
for x in range(3):
self.move_arm_action_client.send_goal(goal)
while True:
cur_state = self.move_arm_action_client.get_state()
if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
r.sleep()
end_state = self.move_arm_action_client.get_state()
if(end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED): break
final_state = self.move_arm_action_client.get_state()
self.assertEqual(final_state, actionlib_msgs.msg.GoalStatus.SUCCEEDED)
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
