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

update libraries

common/Motion/config.py

@@ -1,14 +1,14 @@
 #################### SYSTEM CONFIGURATION VARIABLES #######################
 
-#change this to None to use the default mode linked to by libebolabot.so
+#change this to "client" to use the MotionServer_X program
 #change this to "kinematic" to test in kinematic mode
 #change this to "physical" if you are ready to test on the real robot
 #mode = 'client'
 #mode = 'kinematic'
 mode = 'physical'
 
-#relative path, assumes everything is run from the Ebolabot directory
-klampt_model = "klampt_models/ebolabot_col.rob"
+#relative path, assumes everything is run from the common directory
+klampt_model = "klampt_models/baxter_with_parallel_gripper_col.rob"
 #klampt_model = "klampt_models/hstar_amp1_col.rob"
 
 #Motion server address.  Use this if you are using a server on another
@@ -27,8 +27,8 @@ def setup(parse_sys=True):
     global mode,klampt_model,motion_server_addr
     import os
     import motion
-    ebolabot_root = os.getenv("EBOLABOT_PATH","./")
+    motion_root = os.getenv("MOTION_API_PATH","./")
     if parse_sys:
         parse_args()
     print "Loading Motion Module model",klampt_model
-    motion.setup(mode=mode,klampt_model=klampt_model,libpath=ebolabot_root,server_addr=motion_server_addr)
+    motion.setup(mode=mode,klampt_model=klampt_model,libpath=motion_root,server_addr=motion_server_addr)

common/Motion/motion.h

@@ -80,11 +80,11 @@ APIENTRY BOOL publishState(const char* system_state_addr="tcp://localhost:4568")
 
 ////// OVERALL ROBOT ///////
 
-///Starts up the robot
+///Starts up the robot and motion API
 APIENTRY BOOL sendStartup();
 ///Returns true if the robot is started
 APIENTRY BOOL isStarted();
-///Shuts down the robot
+///Shuts down the motion API.  (Does not shut down the robot)
 APIENTRY BOOL sendShutdown();
 ///Gets the time since startup
 APIENTRY double getTime();

common/Motion/motion.py

@@ -34,7 +34,7 @@
 from ctypes import *
 import json
 import platform
-
+import signal
 
 LEFT=0
 RIGHT=1
@@ -570,7 +570,14 @@ def getKlamptModel(self):
         return p.value
     def startup(self):
         """Starts up the robot"""
-        return motion_lib.sendStartup()
+        res = motion_lib.sendStartup()
+        if res == False: return False
+        #overrides the default Ctrl+C behavior which kills the program
+        def interrupter(x,y):
+            self.shutdown()
+            raise KeyboardInterrupt()
+        signal.signal(signal.SIGINT,interrupter)
+        return res
     def shutdown(self):
         """Shuts down the robot"""
         return motion_lib.sendShutdown()

common/Motion/motion_common.h

@@ -120,15 +120,12 @@ BOOL publishState(const char* system_state_addr)
 }
 
 void my_interrupt_handler(int s){
-  printf("Caught signal %d\n",s);
+  printf("Caught signal %d, stopping robot motion\n",s);
   gData.mutex.try_lock();
   gData.mutex.unlock();
-  printf("stopMotion\n");
   stopMotion();
-  printf("sendShutdown\n");
   sendShutdown();
   exit(1); 
-
 }
 
 ///Starts up the robot
@@ -148,6 +145,7 @@ BOOL sendStartup()
   sigIntHandler.sa_handler = my_interrupt_handler;
   sigemptyset(&sigIntHandler.sa_mask);
   sigIntHandler.sa_flags = 0;
+  sigIntHandler.sa_sigaction = 0;
 
   sigaction(SIGINT, &sigIntHandler, NULL);
 
@@ -167,6 +165,15 @@ BOOL sendShutdown()
   stopMotion();
   gData.kill = true;
   ThreadJoin(gControllerUpdateThread);
+
+  struct sigaction sigIntHandler;
+  sigIntHandler.sa_handler = SIG_DFL;
+  sigemptyset(&sigIntHandler.sa_mask);
+  sigIntHandler.sa_flags = 0;
+  sigIntHandler.sa_sigaction = 0;
+
+  sigaction(SIGINT, &sigIntHandler, NULL);
+
   return true;
 }
 
@@ -1035,21 +1042,27 @@ BOOL isMotionQueueEnabled(int limb)
 ///Returns true the limb is moving according to the motion queue
 BOOL isMotionQueueMoving(int limb)
 {
-  ScopedLock lock(gData.mutex);
-  if(limb == LEFT) return gData.robotState.leftLimb.motionQueueActive && (gData.robotState.leftLimb.motionQueue.TimeRemaining() >= 0);
-  else if(limb == RIGHT) return gData.robotState.rightLimb.motionQueueActive && (gData.robotState.rightLimb.motionQueue.TimeRemaining() >= 0);
+  if(limb == LEFT) {
+    ScopedLock lock(gData.mutex);
+    return gData.robotState.leftLimb.motionQueueActive && (gData.robotState.leftLimb.motionQueue.TimeRemaining() >= 0);
+  }
+  else if(limb == RIGHT) {
+    ScopedLock lock(gData.mutex);
+    return gData.robotState.rightLimb.motionQueueActive && (gData.robotState.rightLimb.motionQueue.TimeRemaining() >= 0);
+  }
   else return isMotionQueueMoving(LEFT) && isMotionQueueMoving(RIGHT);
 }
 ///Returns the estimated amount of time until the motion queue stops
 double getMotionQueueMoveTime(int limb)
 {
-  ScopedLock lock(gData.mutex);
   if(limb == LEFT) {
+    ScopedLock lock(gData.mutex);
     if(gData.robotState.leftLimb.motionQueueActive)
       return Max(gData.robotState.leftLimb.motionQueue.TimeRemaining(),0.0);
     return 0;
   }
   else if(limb == RIGHT) {
+    ScopedLock lock(gData.mutex);
     if(gData.robotState.rightLimb.motionQueueActive)
       return Max(gData.robotState.rightLimb.motionQueue.TimeRemaining(),0.0);
     return 0;
@@ -1166,7 +1179,7 @@ BOOL _sendMotionQueueRamp(int limb,const double* angles,double speed,bool immedi
     }
   }
   else {
-    printf("sendMotionQueueRamp(): warning, can't ramp arms simultaneously\n");
+    printf("sendMotionQueueRamp(): warning, can't ramp arms simultaneously yet\n");
     return _sendMotionQueueRamp(LEFT,angles,speed,immediate) && _sendMotionQueueRamp(RIGHT,angles+numLimbDofs,speed,immediate);
   }
   return 1;

common/Motion/motion_physical.cpp

@@ -10,6 +10,7 @@
 #include <baxter_core_msgs/JointCommand.h>
 #include <baxter_core_msgs/HeadPanCommand.h>
 #include <baxter_core_msgs/EndEffectorCommand.h>
+#include <baxter_core_msgs/DigitalIOState.h>
 #include <nav_msgs/Odometry.h>
 #include <geometry_msgs/Twist.h>
 #include <reflex_sf_msgs/JointState.h>
@@ -116,6 +117,7 @@ class MyControllerUpdateData : public ControllerUpdateData
   AutoMsg<baxter_core_msgs::HeadState> headState;
   AutoMsg<baxter_core_msgs::EndEffectorState> leftGripperState,rightGripperState;
   AutoMsg<reflex_sf_msgs::JointState> leftReflexGripperState,rightReflexGripperState;
+  AutoMsg<baxter_core_msgs::DigitalIOState> leftCuffState,rightCuffState;
   AutoMsg<nav_msgs::Odometry> baseState;
   Config lastBaseDifferencePos;
   double lastBaseDifferenceTime;
@@ -161,13 +163,17 @@ bool MyControllerUpdateData::MyStartup() {
   nh = new ros::NodeHandle();
   larm_pub = nh->advertise<baxter_core_msgs::JointCommand>(string("robot/limb/left/joint_command"),10,false);
   rarm_pub = nh->advertise<baxter_core_msgs::JointCommand>(string("robot/limb/right/joint_command"),10,false);
-  head_pan_pub = nh->advertise<baxter_core_msgs::HeadPanCommand>(string("robot/head/command_head_pan"),100,false);
+  head_pan_pub = nh->advertise<baxter_core_msgs::HeadPanCommand>(string("robot/head/command_head_pan"),10,false);
   head_nod_pub = nh->advertise<std_msgs::Bool>("robot/head/command_head_nod",10,false);
   base_twist_pub = nh->advertise<geometry_msgs::Twist>("hstar_amp1/base/auto/cmd/ts",10,false);
 
   assemblyState.subscribe(*nh,"robot/state",10);
   jointState.subscribe(*nh,"robot/joint_states",10);
   headState.subscribe(*nh,"robot/head/head_state",10);
+  leftCuffState.state = 0;
+  rightCuffState.state = 0;
+  leftCuffState.subscribe(*nh,"robot/digital_io/left_lower_cuff/state",10);
+  rightCuffState.subscribe(*nh,"robot/digital_io/right_lower_cuff/state",10);
   leftGripperState.subscribe(*nh,"robot/end_effector/left_gripper/state",10);
   rightGripperState.subscribe(*nh,"robot/end_effector/right_gripper/state",10);
 
@@ -441,12 +447,18 @@ bool MyControllerUpdateData::MyProcessSensors()
 	robotState.rightLimb.sensedEffort[k] = jointState->effort[i];
       }      
     }
-    if(robotState.leftLimb.commandedConfig.empty()) {
+    if(robotState.leftLimb.commandedConfig.empty() || leftCuffState.state==1 /*startup or cuff pressed, reset commanded*/) {
+      SetLimbGovernor(LEFT,LimbState::Normal);
+      robotState.leftLimb.controlMode = LimbState::POSITION;
+      robotState.leftLimb.sendCommand = false;
       robotState.leftLimb.commandedConfig = robotState.leftLimb.sensedConfig;
       robotState.leftLimb.commandedVelocity = robotState.leftLimb.sensedVelocity;
       robotState.leftLimb.commandedEffort = robotState.leftLimb.sensedEffort;
     }
-    if(robotState.rightLimb.commandedConfig.empty()) {
+    if(robotState.rightLimb.commandedConfig.empty() || rightCuffState.state==1 /*startup or cuff pressed, reset commanded*/) {
+      SetLimbGovernor(RIGHT,LimbState::Normal);
+      robotState.rightLimb.controlMode = LimbState::POSITION;
+      robotState.rightLimb.sendCommand = false;
       robotState.rightLimb.commandedConfig = robotState.rightLimb.sensedConfig;
       robotState.rightLimb.commandedVelocity = robotState.rightLimb.sensedVelocity;
       robotState.rightLimb.commandedEffort = robotState.rightLimb.sensedEffort;