q_kdl = kdl.JntArray(n) for i in range(n): q_kdl[i] = q[i] return q_kdl if __name__ == '__main__': roslib.load_manifest('hrl_pr2_lib') import hrl_pr2_lib.pr2_arms as pa import hrl_lib.viz as hv from visualization_msgs.msg import Marker rospy.init_node('kdl_pr2_test') marker_pub = rospy.Publisher('/kdl_pr2_arms/viz_marker', Marker) pr2_arms = pa.PR2Arms(gripper_point=(0.,0.,0.)) pr2_kdl = PR2_arm_kdl() r_arm, l_arm = 0, 1 while not rospy.is_shutdown(): q = pr2_arms.get_joint_angles(r_arm) p, r = pr2_kdl.FK_all('right_arm', q, 7) m = hv.create_frame_marker(p, r, 0.3, 'torso_lift_link') time_stamp = rospy.Time.now() m.header.stamp = time_stamp marker_pub.publish(m) rospy.sleep(0.1)
if q == None: return None n = len(q) q_kdl = kdl.JntArray(n) for i in range(n): q_kdl[i] = q[i] return q_kdl if __name__ == '__main__': roslib.load_manifest('hrl_pr2_lib') import hrl_pr2_lib.pr2_arms as pa import hrl_lib.viz as hv from visualization_msgs.msg import Marker rospy.init_node('kdl_pr2_test') marker_pub = rospy.Publisher('/kdl_pr2_arms/viz_marker', Marker) pr2_arms = pa.PR2Arms(gripper_point=(0., 0., 0.)) pr2_kdl = PR2_arm_kdl() r_arm, l_arm = 0, 1 while not rospy.is_shutdown(): q = pr2_arms.get_joint_angles(r_arm) p, r = pr2_kdl.FK_all('right_arm', q, 7) m = hv.create_frame_marker(p, r, 0.3, 'torso_lift_link') time_stamp = rospy.Time.now() m.header.stamp = time_stamp marker_pub.publish(m) rospy.sleep(0.1)
if False: jep = [0.] * 7 rospy.loginfo('Going to home location.') raw_input('Hit ENTER to go') pr2_arms.set_jep(arm, jep, duration=2.) if False: # testing FK by publishing a frame marker. marker_pub = rospy.Publisher('/pr2_kdl/ee_marker', Marker) pr2_kdl.set_tooltip(arm, np.matrix([0.15, 0., 0.]).T) rt = rospy.Rate(100) rospy.loginfo('Starting the maker publishing loop.') while not rospy.is_shutdown(): q = pr2_arms.get_joint_angles(arm) p, rot = pr2_kdl.FK_all(arm, q) m = hv.create_frame_marker(p, rot, 0.15, '/torso_lift_link') m.header.stamp = rospy.Time.now() marker_pub.publish(m) rt.sleep() if False: # testing Jacobian by printing KDL and my own Jacobian at the # current configuration. while not rospy.is_shutdown(): q = pr2_arms.get_joint_angles(arm) J_kdl = pr2_kdl.Jac(arm , q) p, rot = pr2_kdl.FK_all(arm, q) J_adv = pr2_kdl.Jacobian(arm, q, p) print J_adv.shape diff_J = J_kdl - J_adv print 'difference between KDL and adv is:'