def get_fk(self, fk_links, robot_state=None, frame_id=None):
'''
Solves forward kinematics.
In general, it is better to use the planning scene or the state transformer get_transform function between
robot links rather than this function.
**Args:**
**fk_links ([string]):** Links for which you want FK solutions
*robot_state (arm_navigation_msgs.mgs.RobotState):* The state of the robot during forward kinematics.
If no state is passed in the state in the planning scene will be used.
*frame_id (string):* The frame ID in which you want the returned poses. Note that the FK service may use
TF so we recommend only using the robot frame. If no frame is specified, the robot frame is used.
**Returns:**
A list of geometry_msgs.msg.PoseStamped corresponding to forward kinematic solutions for fk_links.
**Raises:**
**rospy.ServiceException:** if there is a problem with the service call
**Warning:**
Calls a service which may use TF! Recommended that you only ask for poses in the robot frame.
'''
req = GetPositionFKRequest()
req.header.frame_id = frame_id
if not frame_id:
req.header.frame_id = self._psi.robot_frame
req.header.stamp = rospy.Time(0)
req.robot_state = robot_state
if not robot_state:
req.robot_state = self._psi.get_robot_state()
req.fk_link_names = fk_links
res = self._fk_service(req)
if res.error_code.val != res.error_code.SUCCESS or not res.pose_stamped:
raise ArmNavError('Forward kinematics failed',
error_code=res.error_code)
return res.pose_stamped
def get_fk(self, fk_links, robot_state=None, frame_id=None):
'''
Solves forward kinematics.
In general, it is better to use the planning scene or the state transformer get_transform function between
robot links rather than this function.
**Args:**
**fk_links ([string]):** Links for which you want FK solutions
*robot_state (arm_navigation_msgs.mgs.RobotState):* The state of the robot during forward kinematics.
If no state is passed in the state in the planning scene will be used.
*frame_id (string):* The frame ID in which you want the returned poses. Note that the FK service may use
TF so we recommend only using the robot frame. If no frame is specified, the robot frame is used.
**Returns:**
A list of geometry_msgs.msg.PoseStamped corresponding to forward kinematic solutions for fk_links.
**Raises:**
**rospy.ServiceException:** if there is a problem with the service call
**Warning:**
Calls a service which may use TF! Recommended that you only ask for poses in the robot frame.
'''
req = GetPositionFKRequest()
req.header.frame_id = frame_id
if not frame_id:
req.header.frame_id = self._psi.robot_frame
req.header.stamp = rospy.Time(0)
req.robot_state = robot_state
if not robot_state:
req.robot_state = self._psi.get_robot_state()
req.fk_link_names = fk_links
res = self._fk_service(req)
if res.error_code.val != res.error_code.SUCCESS or not res.pose_stamped:
raise ArmNavError('Forward kinematics failed', error_code=res.error_code)
return res.pose_stamped
