Merge pull request duke-iml#2 from duke-iml/master

update from master

README.md

@@ -38,7 +38,7 @@ Workstation Setup:
 Simple test On Lab Workstation:
 -----------------------------------------
 Turn on baxter by pushing the little white button on its back once. 
-After you log in to ubuntu, open a terminal and run "cd /home/ece_user/ros_ws" then ".baxter.sh". Then you can try to follow this link from step 2: http://sdk.rethinkrobotics.com/wiki/Hello_Baxter. Have fun!
+After you log in to ubuntu, open a terminal and run "cd ros_ws" then ".baxter.sh". Then you can try to follow this link from step 2: http://sdk.rethinkrobotics.com/wiki/Hello_Baxter. Have fun!
 
 
 To turn off baxter, push the same button once.

common/Motion/motion.h

@@ -258,6 +258,8 @@ APIENTRY BOOL sendEndEffectorPositionDrive(int limb,const double* vel);
 ///instantaneous velocity which is translated into joint velocities, which drifts
 ///away from the integrated Cartesian motion over time.
 APIENTRY BOOL sendEndEffectorDrive(int limb,const double* angVel,const double* vel);
+///Returns true if the limb is in end effector drive mode
+APIENTRY BOOL isEndEffectorDriveEnabled(int limb);
 
 
 

common/Motion/motion.py

@@ -118,9 +118,11 @@ def __init__(self,limb):
         self.temp_p = cast(self.temp,POINTER(c_double))
         self.itemp = None
     def mode(self):
-        """Returns 'position', 'velocity', 'effort', 'raw_position', 'motion_queue',
+        """Returns 'position', 'velocity', 'effort', 'raw_position',
+        'motion_queue', 'end_effector_drive',
         or 'planner' depending on the current operational mode of the limb."""
         if motion_lib.isPlannerEnabled(self.limb): return 'planner'
+        elif motion_lib.isEndEffectorDriveEnabled(self.limb): return 'end_effector_drive'
         elif motion_lib.isMotionQueueEnabled(self.limb): return 'motion_queue'
         elif motion_lib.isLimbPositionMode(self.limb): return 'position'
         elif motion_lib.isLimbVelocityMode(self.limb): return 'velocity'

common/Motion/motion_common.h

@@ -838,6 +838,12 @@ APIENTRY BOOL sendEndEffectorDrive(int limb,const double* angVel,const double* v
   return 1;
 }
 
+BOOL isEndEffectorDriveEnabled(int limb)
+{
+  if(limb == LEFT) return gData.robotState.leftLimb.governor == LimbState::EndEffectorDrive;
+  else if(limb == RIGHT) return gData.robotState.rightLimb.governor == LimbState::EndEffectorDrive;
+  else return gData.robotState.leftLimb.governor == LimbState::EndEffectorDrive && gData.robotState.rightLimb.governor == LimbState::EndEffectorDrive;
+}
 
 ////// GRIPPER ///////
 

common/Motion/motion_kinematic.cpp

@@ -90,10 +90,12 @@ class MyControllerUpdateData : public ControllerUpdateData
     robotState.rightLimb.senseUpdateTime = t;
 
     //base moving flag
-    if(robotState.base.velocity.norm() > 1e-3 || robotState.base.sendCommand)
-      robotState.base.moving = true;
-    else
-      robotState.base.moving = false;
+    if(robotState.base.enabled) {
+      if(robotState.base.velocity.norm() > 1e-3 || robotState.base.sendCommand)
+	robotState.base.moving = true;
+      else
+	robotState.base.moving = false;
+    }
 
     lastSensorTime = t;
     return true;

common/Testing/simpletest.py

@@ -0,0 +1,13 @@
+from Motion import motion
+
+motion.setup(mode='client',klampt_model='klampt_models/baxter_with_parallel_grippers',libpath='.')
+
+motion.robot.startup()
+try:
+    while True:
+        raw_input()
+        print motion.robot.left_limb.sensedPosition()
+except KeyboardInterrupt:
+    motion.robot.stopMotion()
+
+

common/Testing/widget_control.py

@@ -42,7 +42,7 @@ def display(self):
         #the current example draws the sensed robot in grey and the
         #commanded configurations in transparent green
 
-        #this line with draw the world
+        #this line will draw the robot's sensed configuration in the world
         robot = motion.robot
         q = robot.getKlamptSensedPosition()
         self.world.robot(0).setConfig(q)