def baxter_ik_move(self, limb, rpy_pose):
quaternion_pose = self.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
iterate_IK = True
n_iterations = 0
ik_request.pose_stamp.append(quaternion_pose)
while iterate_IK:
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message,))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
if ik_response.isValid[0]:
print("PASS: Valid joint configuration found")
# convert response to joint position control dictionary
limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
# move limb
if self.limb == limb:
self.limb_interface.move_to_joint_positions(limb_joints)
else:
self.other_limb_interface.move_to_joint_positions(limb_joints)
iterate_IK = False
else:
currentJointState = rospy.wait_for_message("/robot/joint_states",JointState)
newJointState=copy.copy(currentJointState)
seed_angle_val=JointState()
seed_angle_val.header.stamp = rospy.Time.now()
current_angle = []
name_val = []
for i in range (2,9):
temp_name = copy.copy(currentJointState.name[i])
temp_angle = copy.copy(currentJointState.position[i])
temp_angle = temp_angle+(random.random()-0.5)/10
name_val.append(temp_name)
current_angle.append(temp_angle)
seed_angle_val.name=name_val
seed_angle_val.position=current_angle
seed_angle_list=[seed_angle_val]
ik_request.seed_angles = seed_angle_list
n_iterations=n_iterations+1
print n_iterations
if n_iterations > 50:
# display invalid move message on head display
self.splash_screen("Invalid IK", "Solution")
#sys.exit("ERROR - baxter_ik_move - No valid joint configuration found")
print 'IK did not converge after %d iterations' %n_iterations
return
z=0.00,
),
orientation=Quaternion(
x=0.0,
y=0.71,
z=0.0,
w=-0.71,
),
),
),
}
ikreq_r = SolvePositionIKRequest()
ikreq_r.pose_stamp = [poses['right']]
seed = random_seed()
ikreq_r.seed_angles = seed
ikreq_r.seed_mode = 1
print "IKREQ_FIXED = ", ikreq_r
#print "IKREQ_R : ",ikreq_r
try:
rospy.wait_for_service(ns_r, 5.0)
resp_r = iksvc_r(ikreq_r)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return
if (resp_r.isValid[0]):
print("RIGHT_SUCCESS - Valid Joint Solution Found:")
# Format solution into Limb API-compatible dictionary
limb_joints_r = dict(
