visualization of rosie left arm Jacobian.

motion_viz/left_arm_jac.launch

@@ -0,0 +1,15 @@
+<launch>
+
+ <node name="robot_jac" type="robot_jac.py" pkg="motion_viz">
+  <remap from="constraint_state" to="/feature_controller/constraint_state"/>
+ </node>
+
+ <node name="twist_viz" type="twist_viz.py" pkg="motion_viz">
+  <remap from="twist" to="/jacobian"/>
+  <param name="ref_frame" value="base_link"/>
+  <param name="ref_point" value="base_link"/>
+  <param name="target_frame" value="left_arm_4_link"/>
+  <param name="max_length" value="0.3"/>
+ </node>
+
+</launch>

motion_viz/robot_jac.py

@@ -0,0 +1,24 @@
+#!/usr/bin/python
+
+# extract the robot Jacobian from constraint state message
+
+import roslib
+roslib.load_manifest('motion_viz')
+import rospy
+
+from motion_viz.msg import Jacobian
+from constraint_msgs.msg import ConstraintState
+
+jac = Jacobian()
+
+def callback(msg):
+  jac.columns = msg.robot_jacobian
+  pub.publish(jac)
+
+rospy.init_node('robot_jac')
+
+pub = rospy.Publisher('jacobian', Jacobian)
+sub = rospy.Subscriber('constraint_state', ConstraintState, callback)
+
+rospy.spin()
+

motion_viz/twist_viz.py

@@ -28,7 +28,6 @@
 def axis_marker(tw, id = 0, ns = 'twist'):
   """ make a marker message showing the instantaneous
       rotation axis of a twist message"""
-  min_length = 0.03
 
   t = kdl.Twist(kdl.Vector(tw.linear.x,  tw.linear.y,  tw.linear.z),
                 kdl.Vector(tw.angular.x, tw.angular.y, tw.angular.z))
@@ -66,6 +65,9 @@ def axis_marker(tw, id = 0, ns = 'twist'):
   elif lr < min_length:
     direction = direction / lr * min_length
     m = marker.align(m, location - direction, location + direction, 0.02)
+  elif lr > max_length:
+    direction = direction / lr * max_length
+    m = marker.align(m, location - direction, location + direction, 0.02) 
   else:
     #BAH! How do I make this better?
     m = marker.align(m, location - direction, location + direction, 0.02)
@@ -124,6 +126,10 @@ def any_callback(msg):
 rospy.loginfo('ref_frame=%s, ref_point=%s, target_frame=%s'
                % (ref_frame, ref_point, target_frame))
 
+
+min_length = rospy.get_param('~min_length', 0.03)
+max_length = rospy.get_param('~max_length', 0.30)
+
 use_colors = rospy.get_param('~colors', True)
 marker_id = rospy.get_param('marker_id', 1)
 node_name = rospy.get_name()[1:]