def script():
rospy.init_node('forward_kin')
tflistener = tf.TransformListener()
print 'waiting for transform'
tflistener.waitForTransform('r_gripper_tool_frame', 'r_wrist_roll_link', rospy.Time(), rospy.Duration(10))
print 'waiting for services'
rospy.wait_for_service('pr2_right_arm_kinematics/get_fk_solver_info')#, ks.GetKinematicSolverInfo )
rospy.wait_for_service('pr2_right_arm_kinematics/get_fk')#, ks.GetPositionFK)
print 'done init'
r_fk_info = rospy.ServiceProxy('pr2_right_arm_kinematics/get_fk_solver_info', ks.GetKinematicSolverInfo )
r_fk = rospy.ServiceProxy('pr2_right_arm_kinematics/get_fk', ks.GetPositionFK)
resp = r_fk_info()
print 'get_fk_solver_info returned', resp.kinematic_solver_info.joint_names
print 'get_fk_solver_info returned', resp.kinematic_solver_info.limits
print 'get_fk_solver_info returned', resp.kinematic_solver_info.link_names
fk_req = ks.GetPositionFKRequest()
fk_req.header.frame_id = 'torso_lift_link'
fk_req.fk_link_names = ['r_wrist_roll_link']
fk_req.robot_state.joint_state.name = resp.kinematic_solver_info.joint_names
fk_req.robot_state.joint_state.position = [.5 for i in range(len(resp.kinematic_solver_info.joint_names))]
fk_resp = r_fk(fk_req)
rtip_T_wr = tfu.transform('r_gripper_tool_frame', 'r_wrist_roll_link', tflistener)
right_wr = tfu.pose_as_matrix(fk_resp.pose_stamped[0].pose)
rtip_pose = rtip_T_wr * right_wr
print tfu.matrix_as_tf(rtip_pose)
def fk(self,
joint_poses_mat,
frame='torso_lift_link',
sol_link=None,
use_tool_frame=True):
if sol_link == None:
sol_link = self.ik_frame
fk_req = ks.GetPositionFKRequest()
fk_req.header.frame_id = frame
fk_req.fk_link_names = [sol_link]
fk_req.robot_state.joint_state.name = self.fk_info_resp.kinematic_solver_info.joint_names
fk_req.robot_state.joint_state.position = joint_poses_mat.T.A1.tolist()
fk_resp = self._fk(fk_req)
solframe_T_wr = tfu.pose_as_matrix(fk_resp.pose_stamped[0].pose)
if not use_tool_frame:
return solframe_T_wr
else:
#print 'redoing'
self.tflistener.waitForTransform(self.tool_frame, sol_link,
rospy.Time(), rospy.Duration(10))
wr_T_toolframe = tfu.transform(sol_link, self.tool_frame,
self.tflistener)
return solframe_T_wr * wr_T_toolframe
def fk(self, joint_poses_mat, frame='torso_lift_link', sol_link=None, use_tool_frame=True):
if sol_link == None:
sol_link = self.ik_frame
fk_req = ks.GetPositionFKRequest()
fk_req.header.frame_id = frame
fk_req.fk_link_names = [sol_link]
fk_req.robot_state.joint_state.name = self.fk_info_resp.kinematic_solver_info.joint_names
fk_req.robot_state.joint_state.position = joint_poses_mat.T.A1.tolist()
fk_resp = self._fk(fk_req)
solframe_T_wr = tfu.pose_as_matrix(fk_resp.pose_stamped[0].pose)
if not use_tool_frame:
return solframe_T_wr
else:
#print 'redoing'
self.tflistener.waitForTransform(self.tool_frame, sol_link, rospy.Time(), rospy.Duration(10))
wr_T_toolframe = tfu.transform(sol_link, self.tool_frame, self.tflistener)
return solframe_T_wr * wr_T_toolframe
def script():
rospy.init_node('forward_kin')
tflistener = tf.TransformListener()
print 'waiting for transform'
tflistener.waitForTransform('r_gripper_tool_frame', 'r_wrist_roll_link',
rospy.Time(), rospy.Duration(10))
print 'waiting for services'
rospy.wait_for_service('pr2_right_arm_kinematics/get_fk_solver_info'
) #, ks.GetKinematicSolverInfo )
rospy.wait_for_service(
'pr2_right_arm_kinematics/get_fk') #, ks.GetPositionFK)
print 'done init'
r_fk_info = rospy.ServiceProxy(
'pr2_right_arm_kinematics/get_fk_solver_info',
ks.GetKinematicSolverInfo)
r_fk = rospy.ServiceProxy('pr2_right_arm_kinematics/get_fk',
ks.GetPositionFK)
resp = r_fk_info()
print 'get_fk_solver_info returned', resp.kinematic_solver_info.joint_names
print 'get_fk_solver_info returned', resp.kinematic_solver_info.limits
print 'get_fk_solver_info returned', resp.kinematic_solver_info.link_names
fk_req = ks.GetPositionFKRequest()
fk_req.header.frame_id = 'torso_lift_link'
fk_req.fk_link_names = ['r_wrist_roll_link']
fk_req.robot_state.joint_state.name = resp.kinematic_solver_info.joint_names
fk_req.robot_state.joint_state.position = [
.5 for i in range(len(resp.kinematic_solver_info.joint_names))
]
fk_resp = r_fk(fk_req)
rtip_T_wr = tfu.transform('r_gripper_tool_frame', 'r_wrist_roll_link',
tflistener)
right_wr = tfu.pose_as_matrix(fk_resp.pose_stamped[0].pose)
rtip_pose = rtip_T_wr * right_wr
print tfu.matrix_as_tf(rtip_pose)
