def baxter_ik_move(self, rpy_pose):
# quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
quaternion_pose = convert_pose_to_msg(rpy_pose, "base")
node = "ExternalTools/" + self.limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
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,))
print "ERROR - baxter_ik_move - Failed to append pose"
return 1
if (ik_response.isValid[0]):
# convert response to joint position control dictionary
limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
# move limb
self.limb_interface.move_to_joint_positions(limb_joints)
else:
print "ERROR - baxter_ik_move - No valid joint configuration found"
return 2
self.getPose() #Store current pose into self.pose
print "Move Executed"
return -1
#Because we can't get full moveit_commander on the raspberry pi due to memory limitations, rewritten implementation of the required conversion function
baxter_arm_control.py 文件源码
python
阅读 26
收藏 0
点赞 0
评论 0
评论列表
文章目录