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:'
