From 3525094d853807b15655cac5a14fcf3d2cd12d41 Mon Sep 17 00:00:00 2001 From: krishauser Date: Wed, 10 Feb 2016 01:57:07 -0500 Subject: [PATCH] Fixed problems with dual-arm motion queue --- common/Motion/config.py | 14 ++-- common/Motion/motion.h | 4 +- common/Motion/motion.py | 11 ++- common/Motion/motion_common.h | 31 ++++++-- common/Motion/motion_visualizer.py | 114 ----------------------------- 5 files changed, 41 insertions(+), 133 deletions(-) delete mode 100644 common/Motion/motion_visualizer.py diff --git a/common/Motion/config.py b/common/Motion/config.py index 8210178..a535664 100644 --- a/common/Motion/config.py +++ b/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) diff --git a/common/Motion/motion.h b/common/Motion/motion.h index b4e7321..f9a07ef 100644 --- a/common/Motion/motion.h +++ b/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(); diff --git a/common/Motion/motion.py b/common/Motion/motion.py index 20cd959..1145c39 100644 --- a/common/Motion/motion.py +++ b/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() diff --git a/common/Motion/motion_common.h b/common/Motion/motion_common.h index 8ffcf24..006e686 100644 --- a/common/Motion/motion_common.h +++ b/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; diff --git a/common/Motion/motion_visualizer.py b/common/Motion/motion_visualizer.py deleted file mode 100644 index 562eeb7..0000000 --- a/common/Motion/motion_visualizer.py +++ /dev/null @@ -1,114 +0,0 @@ -import motion -from klampt import * -from klampt.glprogram import * -import math -import time -import os - -#################### SYSTEM CONFIGURATION VARIABLES ####################### - -#change this to None to use the default mode linked to by libebolabot.so -#change this to "kinematic" to test in kinematic mode -#change this to "physical" if you are ready to test on the real robot -mode = 'kinematic' - -#assumes Klampt is in the home directory -klampt_model = os.path.join(os.path.expanduser("~"),"gitlibs/Klampt/data/robots/baxter_col.rob") - -######################################################################### - -class MyGLViewer(GLRealtimeProgram): - def __init__(self,world): - GLRealtimeProgram.__init__(self,"My GL program") - self.world = world - self.t0 = time.time() - self.inPosition = False - - def display(self): - #Put your display handler here - #the current example draws the sensed robot in grey and the - #commanded configurations in transparent green - robot = motion.robot - q = robot.getKlamptSensedPosition() - self.world.robot(0).setConfig(q) - self.world.drawGL() - - #draw commanded configuration - glEnable(GL_BLEND) - glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA) - glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5]) - q = robot.getKlamptCommandedPosition() - self.world.robot(0).setConfig(q) - self.world.robot(0).drawGL(False) - glDisable(GL_BLEND) - - def control_loop(self): - robot = motion.robot - #Put your control handler here - #currently moves a joint according to sinusoid - #q = robot.left_limb.commandedPosition() - #print "Left commanded",q - #q[1] = math.sin(time.time() ) - #robot.left_limb.positionCommand(q) - t = time.time()-self.t0 - if robot.left_mq.moving(): - pass - elif not self.inPosition: - q = robot.left_limb.commandedPosition() - q[1] = -1.0; - q[3] = 2.0; - q[5] = 1.0; - print "Sent setRamp command" - robot.left_mq.setRamp(q) - self.inPosition = True - else: - robot.left_ee.velocityCommand([0,0,0],[math.sin(t)*0.2,math.cos(t)*0.2,0]) - return - - def idle(self): - self.control_loop() - glutPostRedisplay() - - def mousefunc(self,button,state,x,y): - #Put your mouse handler here - print "mouse",button,state,x,y - GLRealtimeProgram.mousefunc(self,button,state,x,y) - - def motionfunc(self,x,y): - #Put your mouse motion handler here - GLRealtimeProgram.motionfunc(self,x,y) - - def specialfunc(self,c,x,y): - #Put your keyboard special character handler here - print c,"pressed" - - def keyboardfunc(self,c,x,y): - #Put your keyboard handler here - #the current example toggles simulation / movie mode - print c,"pressed" - if c == 'q': - motion.robot.shutdown() - exit(0) - glutPostRedisplay() - - - -if __name__ == "__main__": - print "motion_visualizer.py: sets up a visualizer for testing Motion" - print "control loops. Press q to exit." - print - - print "Loading Motion Module model",klampt_model - motion.setup(mode=mode,klampt_model=klampt_model) - res = motion.robot.startup() - if not res: - print "Error starting up Motion Module" - exit(1) - time.sleep(0.1) - world = WorldModel() - res = world.readFile(klampt_model) - if not res: - raise RuntimeError("Unable to load Motion Module model "+fn) - - viewer = MyGLViewer(world) - viewer.run()