def get_endeffector_pos(self, pos_only=True):
"""
:param pos_only: only return postion
:return:
"""
fkreq = SolvePositionFKRequest()
joints = JointState()
joints.name = self.ctrl.limb.joint_names()
joints.position = [self.ctrl.limb.joint_angle(j)
for j in joints.name]
# Add desired pose for forward kinematics
fkreq.configuration.append(joints)
fkreq.tip_names.append('right_hand')
try:
rospy.wait_for_service(self.name_of_service, 5)
resp = self.fksvc(fkreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return False
pos = np.array([resp.pose_stamp[0].pose.position.x,
resp.pose_stamp[0].pose.position.y,
resp.pose_stamp[0].pose.position.z])
return pos
评论列表
文章目录