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 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)