def move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts=[], link_padding=[], collision_operations=OrderedCollisionOperations()):
move_arm_client = SimpleActionClient('move_left_arm', MoveArmAction)
move_arm_client.wait_for_server()
rospy.loginfo('connected to move_left_arm action server')
goal = MoveArmGoal()
goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
goal.motion_plan_request.planner_id = ''
goal.motion_plan_request.group_name = 'left_arm'
goal.motion_plan_request.num_planning_attempts = 1
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.motion_plan_request.allowed_contacts = allowed_contacts
goal.motion_plan_request.link_padding = link_padding
goal.motion_plan_request.ordered_collision_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.loginfo('timed out trying to achieve a joint goal')
else:
state = move_arm_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo('action finished: %s' % str(state))
return True
else:
rospy.loginfo('action failed: %s' % str(state))
return False
def move_arm_ompl(x,
y,
z,
ox=0.0,
oy=0.0,
oz=0.0,
ow=1.0,
arm_service="move_right_arm",
shoulder_tolerance=(0.5, 0.3),
frame="odom_combined"):
client = actionlib.SimpleActionClient(arm_service, MoveArmAction)
client.wait_for_server()
goal = MoveArmGoal()
goal.disable_ik = True
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 10
goal.motion_plan_request.allowed_planning_time = roslib.rostime.Duration(5)
goal.motion_plan_request.planner_id = ""
addPositionConstraint(goal, x, y, z, frame)
addOrientationConstraint(goal, ox, oy, oz, ow, frame)
constraint = JointConstraint()
constraint.joint_name = "r_shoulder_lift_joint"
constraint.position = -0.05
constraint.tolerance_above = shoulder_tolerance[0]
constraint.tolerance_below = shoulder_tolerance[1]
constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.joint_constraints.append(
constraint)
client.send_goal(goal, done_cb=move_arm_completed_cb)
print client.wait_for_result()
def move_arm_ompl(x, y, z, ox=0.0, oy=0.0, oz=0.0, ow=1.0, arm_service = "move_right_arm", shoulder_tolerance=(0.5, 0.3), frame="odom_combined"):
client = actionlib.SimpleActionClient(arm_service, MoveArmAction)
client.wait_for_server()
goal = MoveArmGoal()
goal.disable_ik = True
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 10
goal.motion_plan_request.allowed_planning_time = roslib.rostime.Duration(5)
goal.motion_plan_request.planner_id = ""
addPositionConstraint(goal, x, y, z, frame)
addOrientationConstraint(goal, ox, oy, oz, ow, frame)
constraint = JointConstraint()
constraint.joint_name = "r_shoulder_lift_joint"
constraint.position = -0.05
constraint.tolerance_above = shoulder_tolerance[0]
constraint.tolerance_below = shoulder_tolerance[1]
constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.joint_constraints.append(constraint)
client.send_goal(goal, done_cb=move_arm_completed_cb)
print client.wait_for_result()
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 getJointConstraints(goal):
joint_names = [
"r_shoulder_pan_joint", "r_shoulder_lift_joint",
"r_upper_arm_roll_joint", "r_elbow_flex_joint", "r_forearm_roll_joint",
"r_wrist_flex_joint", "r_wrist_roll_joint"
]
joint_constraints = []
for joint_name in joint_names:
constraint = JointConstraint()
constraint.joint_name = joint_name
constraint.position = 0
constraint.tolerance_above = 0.1
constraint.tolerance_below = 0.1
constraint.weight = 1.0
joint_constraints.append(constraint)
joint_constraints[0].position = -2
joint_constraints[3].position = -0.2
joint_constraints[5].position = -0.15
goal.motion_plan_request.goal_constraints.joint_constraints = joint_constraints
def create_motion_plan_request_for_joints(joint_positions):
"""Setup motion plan request.
The length of parameter joint_positions must match the length of global variable JOINT_NAMES.
"""
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = "SchunkArm"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
motion_plan_request.planner_id = ""
motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(JOINT_NAMES))
]
for i in range(len(JOINT_NAMES)):
motion_plan_request.goal_constraints.joint_constraints[
i].joint_name = JOINT_NAMES[i]
motion_plan_request.goal_constraints.joint_constraints[
i].position = joint_positions[i]
motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_above = 0.01
motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_below = 0.01
return motion_plan_request
def move_arm_ompl(x, y, z):
service = "move_right_arm"
client = actionlib.SimpleActionClient(service, MoveArmAction)
client.wait_for_server()
goal = MoveArmGoal()
goal.disable_ik = True
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 5
goal.motion_plan_request.allowed_planning_time = roslib.rostime.Duration(5)
goal.motion_plan_request.planner_id = ""
addPositionConstraint(goal, x, y, z)
addOrientationConstraint(goal)
'''
constraint = JointConstraint()
constraint.joint_name = "r_upper_arm_roll_joint"
constraint.position = -2
constraint.tolerance_above = 0.1
constraint.tolerance_below = 0.1
constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.joint_constraints.append(constraint)
'''
constraint = JointConstraint()
constraint.joint_name = "r_shoulder_lift_joint"
constraint.position = 0.4
constraint.tolerance_above = 0.2
constraint.tolerance_below = 0.2
constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.joint_constraints.append(
constraint)
client.send_goal(goal, done_cb=move_arm_completed_cb)
client.wait_for_result()
def getJointConstraints(goal):
joint_names = ["r_shoulder_pan_joint", "r_shoulder_lift_joint",
"r_upper_arm_roll_joint",
"r_elbow_flex_joint",
"r_forearm_roll_joint",
"r_wrist_flex_joint",
"r_wrist_roll_joint"]
joint_constraints = []
for joint_name in joint_names:
constraint = JointConstraint()
constraint.joint_name = joint_name
constraint.position = 0
constraint.tolerance_above = 0.1
constraint.tolerance_below = 0.1
constraint.weight = 1.0
joint_constraints.append(constraint)
joint_constraints[0].position = -2
joint_constraints[3].position = -0.2
joint_constraints[5].position = -0.15
goal.motion_plan_request.goal_constraints.joint_constraints = joint_constraints
def move_arm_ompl(x, y, z):
service = "move_right_arm"
client = actionlib.SimpleActionClient(service, MoveArmAction)
client.wait_for_server()
goal = MoveArmGoal()
goal.disable_ik = True
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 5
goal.motion_plan_request.allowed_planning_time = roslib.rostime.Duration(5)
goal.motion_plan_request.planner_id = ""
addPositionConstraint(goal, x, y, z)
addOrientationConstraint(goal)
'''
constraint = JointConstraint()
constraint.joint_name = "r_upper_arm_roll_joint"
constraint.position = -2
constraint.tolerance_above = 0.1
constraint.tolerance_below = 0.1
constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.joint_constraints.append(constraint)
'''
constraint = JointConstraint()
constraint.joint_name = "r_shoulder_lift_joint"
constraint.position = 0.4
constraint.tolerance_above = 0.2
constraint.tolerance_below = 0.2
constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.joint_constraints.append(constraint)
client.send_goal(goal, done_cb=move_arm_completed_cb)
client.wait_for_result()
def move_arm_joint(self, arm, joint_angles):
self.set_planning_scene()
rospy.loginfo(
"asking move_arm to send the %s arm to go to angles: %s" %
(arm, self.pplist(joint_angles)))
goal = self.create_move_arm_goal(arm)
#add the joint angles as a joint constraint
for (joint_name, joint_angle) in zip(self.joint_names[arm],
joint_angles):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_name
joint_constraint.position = joint_angle
joint_constraint.tolerance_below = .1
joint_constraint.tolerance_above = .1
goal.motion_plan_request.goal_constraints.joint_constraints.append(
joint_constraint)
#send the goal off to move arm
result = self.send_move_arm_goal(arm, goal)
return result
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 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 = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
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
for z in range(2):
min_dist = 1000
if (z % 2 == 0):
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = -2.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 = 0.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
for x in range(3):
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
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
#getting right finger tip link in base_link frame
t = self.tf.getLatestCommonTime(
"/base_link", "/r_gripper_r_finger_tip_link")
finger_point = PointStamped()
finger_point.header.frame_id = "/r_gripper_r_finger_tip_link"
finger_point.header.stamp = t
finger_point.point.x = 0
finger_point.point.y = 0
finger_point.point.z = 0
finger_point_base = self.tf.transformPoint(
"base_link", finger_point)
distance = math.sqrt(
math.pow(finger_point_base.point.x - .6, 2) +
math.pow(finger_point_base.point.y + .6, 2))
# pole is .1 in diameter
distance -= .1
if distance < min_dist:
rospy.loginfo("X: %g Y: %g Dist: %g",
finger_point_base.point.x,
finger_point_base.point.y, distance)
min_dist = distance
r.sleep()
end_state = self.move_arm_action_client.get_state()
if (end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED):
break
rospy.loginfo("Min dist %d is %g", z, min_dist)
#should be a .5 buffer, allowing .1 buffer
self.failIf(min_dist < (allow_padd - extra_buffer))
final_state = self.move_arm_action_client.get_state()
self.assertEqual(final_state,
actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def testAllowedNotAllowedInitialContact(self):
#adding object in collision with base
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "base_object"
obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .1
obj2.shapes[0].dimensions[1] = .1
obj2.shapes[0].dimensions[2] = .1
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = 0
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
self.obj_pub.publish(obj2)
rospy.sleep(5.)
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 = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
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
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = -2.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
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
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
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)
# but we can still overwrite
coll = CollisionOperation()
coll.object1 = "base_link"
coll.object2 = coll.COLLISION_SET_OBJECTS
coll.operation = coll.ENABLE
goal.motion_plan_request.ordered_collision_operations.collision_operations.append(
coll)
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = 0.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
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
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
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
goal.motion_plan_request.group_name = 'left_arm'
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
links = ('L7_wrist_roll_link', 'L6_wrist_pitch_link',
'L5_forearm_roll_link', 'L4_elbow_flex_link',
'L3_upperarm_roll_link', 'L2_shoulder_pan_link',
'L1_shoulder_pitch_link', 'L8_left_finger_link ',
'L9_right_finger_link')
joints = ('shoulder_pitch_joint', 'shoulder_pan_joint',
'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint',
'wrist_pitch_joint', 'wrist_roll_joint')
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint(j, 0.0, 0.1, 0.1, 0.0) for j in joints
]
goal.planning_scene_diff.link_padding = [
LinkPadding(l, 0.0) for l in links
]
init = (0.0, ) * 7
#pick = (0.08297109,-1.29328384, 0.01069185,-0.14805498, 0.14074285, 1.67430316, -1.56874745)
tucked = (-1.9715, -1.7406, 0.0213, -0.1807, -1.8408, 1.0840, 0.1483)
tucked1 = (-1.751, -1.820, -0.084, -0.214, -1.815, 1.089, 0.031)
tucked2 = (-1.8497, -1.8508, 0.0171, -0.2459, -1.9839, 1.0329, 0.1790)
#tucked = (-1.837, -1.823, 0.175, -0.195, -1.861, 1.089, 0.128)
sample1 = (0.0216, -1.1143, 0.1235, 0.9809, 2.6989, -1.7704, 0.2139)
sample2 = (-0.7155, 0.0364, 1.0405, 0.1331, 0.4470, -0.3615, -0.4405)
pick = (0.10373755, -0.25866649, 0.02566044, -0.1677145, -0.32607513,
1.3214246, 1.60355412)
