added publishing for each joint

Author: sjavdani

src/prpy/base/mico.py

@@ -30,6 +30,8 @@
 
 import openravepy
 from manipulator import Manipulator
+from std_msgs.msg import Float64
+import rospy
 
 class Mico(Manipulator):
     def __init__(self, sim,
@@ -67,6 +69,11 @@ def __init__(self, sim,
                 self.GetName(), '', self.GetArmIndices(), 0, True)
             self.servo_simulator = ServoSimulator(self, rate=20,
                                                   watchdog_timeout=0.1)
+            
+        else:
+        #if not simulation, create publishers for each joint
+            self.velocity_topic_names = ['vel_j'+str(i)+'_controller/command' for i in range(1,7)]
+            self.velocity_publishers = [rospy.Publisher(topic_name, Float64) for topic_name in self.velocity_topic_names]
 
     def CloneBindings(self, parent):
         super(Mico, self).CloneBindings(parent)
@@ -99,4 +106,9 @@ def Servo(self, velocities):
             self.GetRobot().GetController().Reset(0)
             self.servo_simulator.SetVelocity(velocities)
         else:
-            raise NotImplementedError('Servo is not implemented.') 
+            for velocity_publisher,velocity in zip(self.velocity_publishers, velocities):
+                velocity_publisher.publish(velocity)
+            #raise NotImplementedError('Servo is not implemented.') 
+
+
+