Fixed problems with dual-arm motion queue

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'
+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,13 +120,12 @@ BOOL publishState(const char* system_state_addr)
 }
 
 void my_interrupt_handler(int s){
-  printf("Caught signal %d, stopping robot\n",s);
+  printf("Caught signal %d, stopping robot motion\n",s);
   gData.mutex.try_lock();
   gData.mutex.unlock();
   stopMotion();
-  //sendShutdown();
+  sendShutdown();
   exit(1); 
-
 }
 
 ///Starts up the robot
@@ -146,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);
 
@@ -165,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;
 }
 
@@ -1033,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;
@@ -1164,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;