def fk_request(self, joint_angles,
end_point='right_hand'):
"""
Forward Kinematics request sent to FK Service
@type joint_angles: dict({str:float})
@param joint_angles: the arm's joint positions
@type end_point: string
@param end_point: name of the end point (should be in URDF)
@return: Forward Kinematics response from FK service
"""
fkreq = SolvePositionFKRequest()
# Add desired pose for forward kinematics
joints = JointState()
joints.name = list(joint_angles.keys())
joints.position = list(joint_angles.values())
fkreq.configuration.append(joints)
# Request forward kinematics from base to end_point
fkreq.tip_names.append(end_point)
try:
resp = self._fksvc(fkreq)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("FK Service call failed: %s" % (e,))
return False
return resp
def fk_service_client(self, joint_pose, limb = "right"):
# returned value contains PoseStanped
# std_msgs/Header header
# geometry_msgs/Pose pose
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/FKService"
fksvc = rospy.ServiceProxy(ns, SolvePositionFK)
fkreq = SolvePositionFKRequest()
joints = JointState()
joints.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
'right_j4', 'right_j5', 'right_j6']
# joints.position = [0.763331, 0.415979, -1.728629, 1.482985,t
# -1.135621, -1.674347, -0.496337]
joints.position = joint_pose
# Add desired pose for forward kinematics
fkreq.configuration.append(joints)
# Request forward kinematics from base to "right_hand" link
# fkreq.tip_names.append('right_hand')
fkreq.tip_names.append('right_gripper_tip')
try:
rospy.wait_for_service(ns, 5.0)
resp = fksvc(fkreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return False
def _FK_request(self,limb,joints):
ns = "ExternalTools/right/PositionKinematicsNode/FKService"
fksvc = rospy.ServiceProxy(ns, SolvePositionFK)
fkreq = SolvePositionFKRequest()
hdr = Header(stamp=rospy.Time.now())
ang = JointState()
ang.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
'right_j4', 'right_j5', 'right_j6']
ang.position = [limb[joints[0]],limb[joints[1]],limb[joints[2]],\
limb[joints[3]],limb[joints[4]],limb[joints[5]],\
limb[joints[6]]]
print ang.position
# Add desired pose for forward kinematics
fkreq.configuration.append(ang)
# Request forward kinematics from base to "right_hand" link
fkreq.tip_names.append('right_hand')
try:
rospy.wait_for_service(ns, 5.0)
resp = fksvc(fkreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return False
def fk_service_client(limb="right"):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/FKService"
fksvc = rospy.ServiceProxy(ns, SolvePositionFK)
fkreq = SolvePositionFKRequest()
joints = JointState()
joints.name = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5',
'right_j6'
]
joints.position = [
0.763331, 0.415979, -1.728629, 1.482985, -1.135621, -1.674347,
-0.496337
]
# Add desired pose for forward kinematics
fkreq.configuration.append(joints)
# Request forward kinematics from base to "right_hand" link
fkreq.tip_names.append('right_hand')
try:
rospy.wait_for_service(ns, 5.0)
resp = fksvc(fkreq)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("Service call failed: %s" % (e, ))
return False
# Check if result valid
if (resp.isValid[0]):
rospy.loginfo("SUCCESS - Valid Cartesian Solution Found")
rospy.loginfo("\nFK Cartesian Solution:\n")
rospy.loginfo("------------------")
rospy.loginfo("Response Message:\n%s", resp)
else:
rospy.loginfo("INVALID JOINTS - No Cartesian Solution Found.")
return True
def jointAngles2Pose(self, joint_angles): ns = "ExternalTools/right/PositionKinematicsNode/FKService" fksvc = rospy.ServiceProxy(ns, SolvePositionFK) fkreq = SolvePositionFKRequest() joints = JointState() print(self.joint_names()) for key in self.joint_names(): joints.name.append(key) joints.position = joint_angles rospy.wait_for_service(ns, 5.0) resp = fksvc(fkreq) rospy.loginfo(resp)
def call(self,
joint_positions,
joint_names=['right_j0', 'right_j1', 'right_j2', 'right_j3',
'right_j4', 'right_j5', 'right_j6'],
tip_names=["right_gripper_tip"]):
"""
Call 'FKService' service
Parameters
----------
joint_positions : list
List of joint angle values (radians) on which to perform forward kinematics.
joint_names : list
List of joint names for the corresponding joint positions.
tip_names : list
List of tip names to which the forward kinematics are calculated.
Returns
-------
: ForwardKinematicsResponse
Returns a ForwardKinematicsResponse built from the SolvePositionFKResponse object returned by the service.
"""
fkreq = SolvePositionFKRequest()
joints = JointState()
joints.name = joint_names
joints.position = joint_positions
# Add desired pose for forward kinematics
fkreq.configuration.append(joints)
# Request forward kinematics from base to "right_hand" link
fkreq.tip_names = tip_names
try:
rospy.wait_for_service(self.ns, 5.0)
resp = self.service(fkreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return ForwardKinematicsResponse(False, None)
def get_endeffector_pose(self):
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
def fk_service_client(limb = "right"):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/FKService"
fksvc = rospy.ServiceProxy(ns, SolvePositionFK)
fkreq = SolvePositionFKRequest()
joints = JointState()
joints.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
'right_j4', 'right_j5', 'right_j6']
joints.position = [0.763331, 0.415979, -1.728629, 1.482985,
-1.135621, -1.674347, -0.496337]
# Add desired pose for forward kinematics
fkreq.configuration.append(joints)
# Request forward kinematics from base to "right_hand" link
fkreq.tip_names.append('right_hand')
try:
rospy.wait_for_service(ns, 5.0)
resp = fksvc(fkreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return False
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_fksrv, 5)
resp = self.fksvc(fkreq)
except (rospy.ServiceException, rospy.ROSException) as 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,
])
if pos_only:
return pos
else:
quat = np.array([
resp.pose_stamp[0].pose.orientation.x,
resp.pose_stamp[0].pose.orientation.y,
resp.pose_stamp[0].pose.orientation.z,
resp.pose_stamp[0].pose.orientation.w
])
zangle = quat_to_zangle(quat)
return np.concatenate([pos, zangle])