Go to the documentation of this file.00001
00002 import numpy as np, math
00003
00004 import roslib; roslib.load_manifest('hrl_cody_arms')
00005 import rospy
00006 import hrl_lib.viz as hv
00007
00008 from visualization_msgs.msg import Marker
00009
00010
00011
00012 def vel_vec_rviz_marker(p, v):
00013 quat = hv.arrow_direction_to_quat(v)
00014 am = hv.single_marker(p, quat, 'arrow', 'torso_lift_link', m_id=1,
00015 duration=0.1)
00016 return am
00017
00018
00019
00020 if __name__ == '__main__':
00021 import cody_arm_client as cac
00022
00023 rospy.init_node('jacobian_tester')
00024 marker_pub = rospy.Publisher('/test_jacobian/viz/markers', Marker)
00025
00026 robot = cac.CodyArmClient('r')
00027 robot.kinematics.set_tooltip(np.matrix([0., 0., -0.16]).T)
00028 while not rospy.is_shutdown():
00029 q = robot.get_joint_angles()
00030 rospy.sleep(0.1)
00031 if q != None:
00032 break
00033 else:
00034 rospy.loginfo('q is None')
00035
00036 jt = 0
00037 point_jt_idx = 3
00038
00039 rt = rospy.Rate(20)
00040
00041 while not rospy.is_shutdown():
00042 q = robot.get_joint_angles()
00043 pos, _ = robot.kinematics.FK(q, point_jt_idx)
00044 J = robot.kinematics.Jacobian(q, pos)
00045 vel_vec = J[0:3, jt]
00046 marker_pub.publish(vel_vec_rviz_marker(pos, vel_vec))
00047 rt.sleep()
00048
00049
00050
00051