diff --git a/common/Motion/config.py b/common/Motion/config.py index 8210178..870c25c 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' -#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 2200f96..006e686 100644 --- a/common/Motion/motion_common.h +++ b/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; diff --git a/common/Motion/motion_physical.cpp b/common/Motion/motion_physical.cpp index ba7c0b6..f442981 100644 --- a/common/Motion/motion_physical.cpp +++ b/common/Motion/motion_physical.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -116,6 +117,7 @@ class MyControllerUpdateData : public ControllerUpdateData AutoMsg headState; AutoMsg leftGripperState,rightGripperState; AutoMsg leftReflexGripperState,rightReflexGripperState; + AutoMsg leftCuffState,rightCuffState; AutoMsg baseState; Config lastBaseDifferencePos; double lastBaseDifferenceTime; @@ -161,13 +163,17 @@ bool MyControllerUpdateData::MyStartup() { nh = new ros::NodeHandle(); larm_pub = nh->advertise(string("robot/limb/left/joint_command"),10,false); rarm_pub = nh->advertise(string("robot/limb/right/joint_command"),10,false); - head_pan_pub = nh->advertise(string("robot/head/command_head_pan"),100,false); + head_pan_pub = nh->advertise(string("robot/head/command_head_pan"),10,false); head_nod_pub = nh->advertise("robot/head/command_head_nod",10,false); base_twist_pub = nh->advertise("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; 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() diff --git a/common/MotionServer_kinematic b/common/MotionServer_kinematic index 5f0c037..2928ca0 100755 Binary files a/common/MotionServer_kinematic and b/common/MotionServer_kinematic differ diff --git a/common/MotionServer_physical b/common/MotionServer_physical index 129f30a..8129729 100755 Binary files a/common/MotionServer_physical and b/common/MotionServer_physical differ diff --git a/common/Testing/GamePad_noGUI.py b/common/Testing/GamePad_noGUI.py deleted file mode 100755 index 9b45bfb..0000000 --- a/common/Testing/GamePad_noGUI.py +++ /dev/null @@ -1,103 +0,0 @@ -""" -Controls Baxter using any game pad / joystick via the logitech and Motion modules. -Designed for Logitech controllers, may work for other controllers too. -Peter Moran, July 2015 - -SETUP: -Before running this script, you must connect to Baxter and the reflex grippers. - -CONTROLS: -Controls are modified when special control buttons are pressed to create combinations, noted below. -Always: - Right trigger -- fully closes right gripper when pressed -No modifier (End effector velocity mode, ie translations): - Right stick x -- move end effector along Baxter's y axis (ie side to side) - Right stick y -- move end effector along Baxter's x axis (ie outward) - Left stick y -- move end effector along Baxter's z axis (ie vertically) -Left bumper (End effector rotational velocity mode): - Right stick x -- rotate end effector about its x axis - Right stick y -- rotate end effector about its y axis - Left stick x -- rotate end effector about its z axis -Right bumper (Joint velocity mode): - Right stick x -- rotate gripper - Right stick y -- bend wrist - Left stick x -- rotate elbow - Left stick y -- bend elbow - -""" - -# Import Modules -import sys -import os -ebolabot_root = os.getenv("EBOLABOT_PATH",".") -sys.path.append(ebolabot_root) -sys.path.append(os.path.join(ebolabot_root,"InputDevices/GamePad") -from Motion import motion -from Motion import config -import logitech - -# Connect to controller -try: - j = logitech.Joystick(0) -except: - print "Joystick not found" - print "Note: Pygame reports there are " + str(logitech.numJoys()) + " joysticks" - exit() - -# Connect to Baxter -robot = motion.setup(mode=config.mode,klampt_model=config.klampt_model,libpath="./") -res = robot.startup() -if not res: - print "Error connecting to robot" - exit() - -print "Moving to neutral configuration" -q = robot.left_limb.commandedPosition() -q[1] = -1.0 -q[3] = 2.0 -q[5] = 1.0 -robot.left_mq.appendLinearRamp(q) -q = robot.right_limb.commandedPosition() -q[1] = -1.0 -q[3] = 2.0 -q[5] = 1.0 -robot.right_mq.setRamp(q) - -while robot.right_mq.moving() or robot.left_mq.moving(): - pass - -while (True): - # Read controller - j.updateState() - rstick = j.get_stick_R() - lstick = j.get_stick_L() - LB = j.get_LB() - RB = j.get_RB() - RT = j.get_RT() - - # Update velocities - if RT > 0.1: - robot.right_gripper.close() - else: - robot.right_gripper.open() - if RB: # Joint velocity mode - jVel = [0, 0, 0, 0, 0, 0, 0] - jVel[6] = rstick[0] / 5 # wrist 2 - jVel[5] = rstick[1] / 5 # wrist 1 - jVel[4] = lstick[0] / 5 # wrist 0 - jVel[3] = lstick[1] / 5 # elbow 1 - motion.robot.left_limb.velocityCommand(jVel) - elif LB: # End effector angular velocity mode - driveVel = [0, 0, 0] - driveAngVel = [0, 0, 0] - driveAngVel[0] = -rstick[0] / 10 # about end effector x - driveAngVel[1] = rstick[1] / 10 # about end effector y - driveAngVel[2] = lstick[0] / 10 # about end effector z - robot.left_ee.driveCommand(driveAngVel, driveVel) - else: # End effector velocity mode - driveVel = [0, 0, 0] - driveAngVel = [0, 0, 0] - driveVel[1] = -rstick[0] / 10 # y - driveVel[0] = -rstick[1] / 10 # x - driveVel[2] = -lstick[1] / 10 # z - robot.left_ee.driveCommand(driveAngVel, driveVel) diff --git a/common/Testing/arm_motion_playback.py b/common/Testing/arm_motion_playback.py new file mode 100644 index 0000000..06d95fd --- /dev/null +++ b/common/Testing/arm_motion_playback.py @@ -0,0 +1,56 @@ +import sys; sys.path.append('.') +import time +from Motion import motion +from Motion import config +from klampt.trajectory import Trajectory + +fn = "arm_motion.traj" +speed = 1 + +print "Plays back an arm motion" +print "Usage: arm_motion_playback.py FILE [SPEED]" +if len(sys.argv) <= 1: + print "Please specify a motion file" + exit(0) +if len(sys.argv) > 2: + speed = float(sys.argv[2]) + +fn = sys.argv[1] +print "Loading motion from",fn +traj = Trajectory() +traj.load(fn) + +if len(traj.milestones[0]) != 14: + print "Error loading arms trajectory, size is not 14" + +#simple motion setup +config.setup(parse_sys=False) + +motion.robot.startup() +try: + print "Moving to start, 20% speed..." + motion.robot.arms_mq.setRamp(traj.milestones[0],speed=0.2) + while motion.robot.arms_mq.moving(): + time.sleep(0.1) + print "Starting motion..." + """ + #this is direct interpolation in Python at 50 Hz + t0 = time.time() + while time.time()-t0 < traj.times[-1]/speed: + q = traj.eval((time.time()-t0)/speed) + motion.robot.left_arm.setPosition(q[0:7]) + motion.robot.right_arm.setPosition(q[7:14]) + time.sleep(0.02) + """ + #this uses the motion queue + t = 0 + q = traj.milestones[0] + for i in range(len(traj.times)): + if traj.times[i] > t: + motion.robot.arms_mq.appendLinear((traj.times[i]-t)/speed,traj.milestones[i]) + t = traj.times[i] + q = traj.milestones[i] + while motion.robot.arms_mq.moving(): + time.sleep(0.1) +except KeyboardInterrupt: + print "Exiting." diff --git a/common/Testing/arm_motion_record.py b/common/Testing/arm_motion_record.py new file mode 100644 index 0000000..1dc862f --- /dev/null +++ b/common/Testing/arm_motion_record.py @@ -0,0 +1,72 @@ +import sys; sys.path.append('.') +import time +from Motion import motion +from Motion import config + +#save at 10Hz, by default. Set to 0 to save by pressing enter +rate = 10.0 +#save here, by default +fn = "arm_motion.traj" +#commanded or sensed? +switch = 'c' + +print "Saves arm motions to disk for later inspection and playback" +print "Usage: arm_motion_record.py TYPE [RATE] [FN]" +print " TYPE can be 'sensed' or 'commanded', or 's' or 'c' for short" +print " [RATE] can be 0 to only save when enter is pressed" +print " [FN] is the file to save to" + +if len(sys.argv) > 1: + switch = sys.argv[1] +if switch == 's': + switch = 'sensed' +if switch == 'c': + switch = 'commanded' +if switch not in ['sensed','commanded']: + print "TYPE argument must be either 'sensed' or 'commanded'" + exit(1) + +if len(sys.argv) > 2: + rate = float(sys.argv[2]) + +if len(sys.argv) > 3: + fn = sys.argv[3] + + +#simple motion setup +config.setup(parse_sys=False) +motion.robot.startup() + +if rate > 0: + print "Saving %s motion at %g Hz to %s "%(switch,rate,fn) +else: + print "Saving %s motion when enter is pressed to %s"%(switch,fn) + +f = open(fn,'w') + +lastt = None +lastq = None +try: + print "Beginning capture loop, press Ctrl+C to quit" + t0 = time.time() + while True: + t = time.time() - t0 + if t < 1e-3: t = 0 + if switch == 'sensed': + q = motion.robot.left_limb.sensedPosition()+motion.robot.right_limb.sensedPosition() + else: + q = motion.robot.left_limb.commandedPosition()+motion.robot.right_limb.commandedPosition() + if q != lastq: + if lastq is not None: + f.write(str(lastt) + '\t' + str(len(lastq)) + '\t'+ ' '.join(str(v) for v in lastq)+'\n') + f.write(str(t) + '\t' + str(len(q)) + '\t'+ ' '.join(str(v) for v in q)+'\n') + lastt = t + lastq = q + if rate <= 0: + raw_input("Press enter to capture > ") + else: + time.sleep(1.0/rate) +except KeyboardInterrupt: + f.close() + print "Saved %g seconds of motion. Exiting."%(time.time()-t0,) + pass diff --git a/common/Testing/base_calibrate.py b/common/Testing/base_calibrate.py deleted file mode 100644 index 6d8fb86..0000000 --- a/common/Testing/base_calibrate.py +++ /dev/null @@ -1,125 +0,0 @@ -from klampt import * -import math -import time -import sys -import os -ebolabot_root = os.getenv("EBOLABOT_PATH",".") -sys.path.append(ebolabot_root) -from Motion import motion -from Motion import config -import csv - -trace_keys=['t','x','y','theta','vx','vy','vtheta','vxcmd','vycmd','vthetacmd'] - -trace = dict((k,[]) for k in trace_keys) - -def update_trace(): - global trace - trace['t'].append(time.time()) - pos = motion.robot.base.odometryPosition() - vel = motion.robot.base.velocity() - velcmd = motion.robot.base.commandedVelocity() - if pos==False: - pos=[0,0,0] - if vel==False: - vel=[0,0,0] - if velcmd==False: - velcmd=[0,0,0] - trace['x'].append(pos[0]) - trace['y'].append(pos[1]) - trace['theta'].append(pos[2]) - trace['vx'].append(vel[0]) - trace['vy'].append(vel[1]) - trace['vtheta'].append(vel[2]) - trace['vxcmd'].append(velcmd[0]) - trace['vycmd'].append(velcmd[1]) - trace['vthetacmd'].append(velcmd[2]) - -def move_constant(cmd,duration): - t0 = time.time() - while time.time()-t0 < duration: - motion.robot.base.moveVelocity(*cmd) - update_trace() - time.sleep(0.01) - motion.robot.base.stop() - -def move_trajectory(cmd,duration): - t0 = time.time() - while time.time()-t0 < duration: - motion.robot.base.moveVelocity(*cmd(time.time()-t0)) - update_trace() - time.sleep(0.01) - motion.robot.base.stop() - -def dosquare(speed,size): - print "Moving at speed",speed,"along square of size",size - side_duration = size / speed - move_constant([speed,0,0],side_duration) - move_constant([0,speed,0],side_duration) - move_constant([-speed,0,0],side_duration) - move_constant([0,-speed,0],side_duration) - -def dowiggle(speed,amount,axis=2): - print "Wiggling at speed",speed,"amount",amount,"on axis",axis - side_duration = amount / speed - cmd = [0,0,0] - cmd[axis] = speed - move_constant(cmd,side_duration) - cmd[axis] = -speed - move_constant(cmd,side_duration) - cmd[axis] = -speed - move_constant(cmd,side_duration) - cmd[axis] = speed - move_constant(cmd,side_duration) - -if __name__ == "__main__": - config.parse_args() - print "base_calibrate.py: gets data about driving the base" - print - print "Loading Motion Module model",config.klampt_model - motion.setup(mode=config.mode,klampt_model=config.klampt_model,libpath="./",) - res = motion.robot.startup() - if not res: - print "Error starting up Motion Module" - exit(1) - time.sleep(0.1) - if not motion.robot.base.enabled(): - print "Base is not enabled..." - exit(1) - - t0 = time.time() - """ - dosquare(0.05,0.3) - move_constant([0,0,0],0.5) - dosquare(0.1,0.3) - move_constant([0,0,0],0.5) - dosquare(0.2,0.3) - move_constant([0,0,0],0.5) - """ - speeds = [0.025,0.05,0.1,0.15,0.2,0.3,0.4] - #wiggle x or y - scale = 1.0 - axis = 1 - for s in speeds: - dowiggle(s,s*scale,axis) - move_constant([0,0,0],0.5) - """ - #wiggle theta - scale = 2.0 - axis = 2 - for s in speeds: - dowiggle(s,s*scale,axis) - move_constant([0,0,0],0.5) - """ - global trace,trace_keys - for i,t in enumerate(trace['t']): - trace['t'][i] = t-t0 - print "Now saving trace to base_calibrate.csv" - with open('base_calibrate.csv', 'w') as csvfile: - writer = csv.writer(csvfile) - writer.writerow(trace_keys) - for i in range(len(trace['t'])): - writer.writerow([trace[k][i] for k in trace_keys]) - print "Done." - motion.robot.shutdown() - print "Saved to base_calibrate.csv." diff --git a/common/Testing/base_control.py b/common/Testing/base_control.py deleted file mode 100644 index 0d9f3c0..0000000 --- a/common/Testing/base_control.py +++ /dev/null @@ -1,182 +0,0 @@ -from klampt import * -from klampt.glprogram import * -import math -import time -import sys -import os -ebolabot_root = os.getenv("EBOLABOT_PATH",".") -sys.path.append(ebolabot_root) -from Motion import motion -from Motion import config - -ebolabot_root = os.getenv("EBOLABOT_PATH",".") -sys.path.append(ebolabot_root) - -from Common.system_config import EbolabotSystemConfig -import threading - - -######################################################################### - -class MyBaseDriveProgram(GLRealtimeProgram): - """Used for driving around the base and testing the Motion base control - functionality""" - def __init__(self,world,name="My GL Widget Program"): - GLRealtimeProgram.__init__(self,name) - self.world = world - self.world.robot(0).setConfig(motion.robot.getKlamptCommandedPosition()) - self.command = [0,0,0] - self.commandType = 'rv' - self.increment = 0.01 - self.incrementang = 0.02 - - def display(self): - #Put your display handler here - 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) - - #draw command / target - if self.commandType == 'op' or self.commandType == 'rp': - tgt = motion.robot.base.odometryTarget() - cmd = self.command - if self.commandType == 'rp': - pos = motion.robot.base.odometryPosition() - c = math.cos(pos[2]) - s = math.sin(pos[2]) - cmdnew = [pos[0]+c*cmd[0]-s*cmd[1],pos[1]+s*cmd[0]+c*cmd[1],pos[2]+cmd[2]] - cmd = cmdnew - glDisable(GL_DEPTH_TEST) - glDisable(GL_LIGHTING) - glPointSize(7.0) - glEnable(GL_POINT_SMOOTH) - glColor3f(0,0.5,1) - glBegin(GL_POINTS) - glVertex3f(cmd[0],cmd[1],0) - glEnd() - glBegin(GL_LINES) - glVertex3f(cmd[0],cmd[1],0) - glVertex3f(cmd[0]+math.cos(cmd[2])*0.4,cmd[1]+math.sin(cmd[2])*0.4,0) - glEnd() - - if tgt: - glColor3f(0.5,0,1) - glBegin(GL_POINTS) - glVertex3f(tgt[0],tgt[1],0) - glEnd() - glBegin(GL_LINES) - glVertex3f(tgt[0],tgt[1],0) - glVertex3f(tgt[0]+math.cos(tgt[2])*0.4,tgt[1]+math.sin(tgt[2])*0.4,0) - glEnd() - glEnable(GL_DEPTH_TEST) - - def specialfunc(self,c,x,y): - #Put your keyboard special character handler here - print c,"pressed" - if c == GLUT_KEY_LEFT: - self.command[1] += self.increment - elif c == GLUT_KEY_RIGHT: - self.command[1] -= self.increment - elif c == GLUT_KEY_UP: - self.command[0] += self.increment - elif c == GLUT_KEY_DOWN: - self.command[0] -= self.increment - self.updateCommand() - self.refresh() - - def updateCommand(self): - if self.commandType == 'rv': - self.sendCommand() - def sendCommand(self): - if self.commandType == 'rv': - motion.robot.base.moveVelocity(*self.command) - elif self.commandType == 'rp': - motion.robot.base.movePosition(*self.command) - elif self.commandType == 'op': - motion.robot.base.moveOdometryPosition(*self.command) - - def keyboardfunc(self,c,x,y): - #Put your keyboard handler here - #the current example toggles simulation / movie mode - print c,"pressed" - c = c.lower() - if c=='h': - self.print_help() - elif c=='s': - self.command = [0,0,0] - motion.robot.base.stop() - elif c==' ': - self.sendCommand() - elif c==',' or c=='<': - self.command[2] += self.incrementang - self.updateCommand() - elif c=='.' or c=='>': - self.command[2] -= self.incrementang - self.updateCommand() - elif c=='v': - print "Switching to velocity mode" - self.command = [0,0,0] - self.commandType = 'rv' - elif c=='r': - print "Switching to relative position mode" - self.command = [0,0,0] - self.commandType = 'rp' - elif c=='o': - print "Switching to odometry position mode" - self.command = list(motion.robot.base.odometryPosition()) - self.commandType = 'op' - elif c=='q': - motion.robot.shutdown() - exit() - self.refresh() - - def idle(self): - GLRealtimeProgram.idle(self) - - def print_help(self): - print "Press h to print this message" - print "Press up/down arrow to move forward/backward arm" - print "Press left/right arrow to move left/right" - print "Press to rotate left/right" - print "Press o to switch to odometry position command mode" - print "Press r to switch to relative position command mode" - print "Press v to switch to velocity command mode" - print "Press [space] to send a position command" - print "Press s to stop motion" - print "Press q to exit." - print - - -if __name__ == "__main__": - config.parse_args() - print "base_control.py: allows driving a base with the Motion module" - print - print "Loading Motion Module model",config.klampt_model - motion.setup(mode=config.mode,klampt_model=config.klampt_model,libpath="./",) - res = motion.robot.startup() - if not res: - print "Error starting up Motion Module" - exit(1) - time.sleep(0.01) - if not motion.robot.base.enabled(): - print "Base is not enabled..." - exit(1) - world = WorldModel() - res = world.readFile(config.klampt_model) - if not res: - raise RuntimeError("Unable to load Klamp't model "+config.klampt_model) - - viewer = MyBaseDriveProgram(world) - viewer.print_help() - viewer.run() - motion.robot.shutdown() diff --git a/common/Testing/collision_test.py b/common/Testing/collision_test.py deleted file mode 100644 index 4dce08e..0000000 --- a/common/Testing/collision_test.py +++ /dev/null @@ -1,123 +0,0 @@ -from klampt import * -from klampt import WidgetSet,RobotPoser -from klampt import vectorops -from klampt.glprogram import * -from klampt.glcommon import GLWidgetPlugin -import math -import time -import sys -import os -ebolabot_root = os.getenv("EBOLABOT_PATH",".") -sys.path.append(ebolabot_root) -from Motion import motion -from Motion import config - -def selfcollision(robot,qstart,qgoal): - """returns whether a self collision is predicted along the route from qstart to qgoal""" - distance = vectorops.distance(qstart,qgoal) - epsilon = math.radians(2) - numsteps = int(math.ceil(distance/epsilon)) - for i in xrange(numsteps+1): - u = float(i)/numsteps - q = robot.interpolate(qstart,qgoal,u) - robot.setConfig(q) - if robot.selfCollides(): - return True - return False - -class GLWidgetProgram(GLPluginProgram): - """A program that uses a widget plugin""" - def __init__(self,name): - GLPluginProgram.__init__(self,"Manual poser") - self.widgetPlugin = GLWidgetPlugin() - def initialize(self): - GLPluginProgram.initialize(self) - self.setPlugin(self.widgetPlugin) - def addWidget(self,widget): - self.widgetPlugin.addWidget(widget) - -class MyGLViewer(GLWidgetProgram): - def __init__(self,world): - GLWidgetProgram.__init__(self,"Manual poser") - - self.world = world - self.robotPoser = RobotPoser(world.robot(0)) - self.addWidget(self.robotPoser) - - robot = world.robot(0) - left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist'] - right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist'] - self.left_arm_link_indices = [robot.link(l).index for l in left_arm_link_names] - self.right_arm_link_indices = [robot.link(l).index for l in right_arm_link_names] - self.firstTimeHack = True - - def display(self): - #Put your display handler here - #the current example draws the sensed robot in grey and the - #commanded configurations in transparent green - - #this line with draw the world - robot = motion.robot - q = robot.getKlamptSensedPosition() - self.world.robot(0).setConfig(q) - self.world.robot(0).drawGL() - GLWidgetProgram.display(self) - - #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 keyboardfunc(self,c,x,y): - #Put your keyboard handler here - #the current example toggles simulation / movie mode - if c == 'h': - print '[space]: send the current posed milestone' - print 'q: clean quit' - elif c == ' ': - q = self.robotPoser.get() - q0 = motion.robot.getKlamptCommandedPosition() - if not self.firstTimeHack and selfcollision(self.world.robot(0),q0,q): - print "Invalid configuration, it self-collides" - else: - self.firstTimeHack = False - robot = motion.robot - robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q)) - robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q)) - qlg = robot.left_gripper.configFromKlampt(q) - qrg = robot.right_gripper.configFromKlampt(q) - robot.left_gripper.command(qlg,[1]*len(qlg),[1]*len(qlg)) - robot.right_gripper.command(qrg,[1]*len(qrg),[1]*len(qrg)) - #robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices]) - #robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices]) - elif c == 'q': - motion.robot.shutdown() - exit(0) - else: - GLWidgetProgram.keyboardfunc(self,c,x,y) - self.refresh() - - -if __name__ == "__main__": - print "collision_test.py: manually sends configurations to the Motion module" - print "Press [space] to send milestones. Press q to exit." - print - - config.setup() - res = motion.robot.startup() - if not res: - print "Error starting up Motion Module" - exit(1) - time.sleep(0.1) - world = WorldModel() - res = world.readFile(config.klampt_model) - if not res: - raise RuntimeError("Unable to load Motion Module model "+fn) - - viewer = MyGLViewer(world) - viewer.run() - motion.robot.shutdown() diff --git a/common/Testing/collision_test_obstacle.py b/common/Testing/collision_test_obstacle.py deleted file mode 100644 index 0f2afcd..0000000 --- a/common/Testing/collision_test_obstacle.py +++ /dev/null @@ -1,330 +0,0 @@ -from klampt import * -from klampt import WidgetSet,RobotPoser -from klampt import vectorops -from klampt.glprogram import * -from klampt.glcommon import GLWidgetPlugin -from klampt.robotcollide import WorldCollider -from klampt.robotsim import Geometry3D -from klampt.robotsim import PointCloud -import math -import time -import os -ebolabot_root = os.getenv("EBOLABOT_PATH",".") -sys.path.append(ebolabot_root) -from Motion import motion -from Motion import config -import rospy -import sensor_msgs.point_cloud2 as pc2 -from sensor_msgs.msg import PointCloud2, PointField -from roslib import message -import struct -from collections import defaultdict -import pcl -from sklearn.neighbors import KDTree -import numpy as np - -def selfcollision(robot,qstart,qgoal): - """returns whether a self collision is predicted along the route from qstart to qgoal""" - distance = vectorops.distance(qstart,qgoal) - epsilon = math.radians(2) - numsteps = int(math.ceil(distance/epsilon)) - for i in xrange(numsteps+1): - u = float(i)/numsteps - q = robot.interpolate(qstart,qgoal,u) - robot.setConfig(q) - if robot.selfCollides(): - return True - return False - -def obstaclecollision(collider,robot,qstart,qgoal): - """returns whether a obstacle collision is predicted along the route from qstart to qgoal""" - #t0 = time.time() - distance = vectorops.distance(qstart,qgoal) - epsilon = math.radians(2) - numsteps = int(math.ceil(distance/epsilon)) - - for i in xrange(numsteps+1): - u = float(i)/numsteps - q = robot.interpolate(qstart,qgoal,u) - robot.setConfig(q) - collisions = collider.robotTerrainCollisions(robot.index) - - if any(collisions): - return collisions - - - #print "Obstacle collision detection done in time", + time.time()-t0 - - - return None - -def bisection(collider,robot,qstart,qgoal): - - distance = vectorops.distance(qstart,qgoal) - epsilon = math.radians(2) - numsteps = int(math.ceil(distance/epsilon)) - - return bisectionhelper(collider,robot,qstart,qgoal,0,numsteps,numsteps) - - -def bisectionhelper(collider,robot,qstart,qgoal,left,right,numsteps): - - mid = (left+right)/2 - u = float(mid)/numsteps - q = robot.interpolate(qstart,qgoal,u) - robot.setConfig(q) - collisions = collider.robotTerrainCollisions(robot.index) - if any(collisions): - return True - if left>=right: - return False - - return bisectionhelper(collider,robot,qstart,qgoal,left,mid-1,numsteps) or bisectionhelper(collider,robot,qstart,qgoal,mid+1,right,numsteps) - - - - - - -class GLWidgetProgram(GLPluginProgram): - """A program that uses a widget plugin""" - def __init__(self,name): - GLPluginProgram.__init__(self,"Manual poser") - self.widgetPlugin = GLWidgetPlugin() - def initialize(self): - GLPluginProgram.initialize(self) - self.setPlugin(self.widgetPlugin) - def addWidget(self,widget): - self.widgetPlugin.addWidget(widget) - -class MyGLViewer(GLWidgetProgram): - def __init__(self,world): - GLWidgetProgram.__init__(self,"Manual poser") - - self.world = world - self.robotPoser = RobotPoser(world.robot(0)) - self.addWidget(self.robotPoser) - - robot = world.robot(0) - left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist'] - right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist'] - self.left_arm_link_indices = [robot.link(l).index for l in left_arm_link_names] - self.right_arm_link_indices = [robot.link(l).index for l in right_arm_link_names] - self.firstTimeHack = True - self.world.makeTerrain('terrain') - - self.subscribe() - self.newPC = None - - def display(self): - #Put your display handler here - #the current example draws the sensed robot in grey and the - #commanded configurations in transparent green - - #this line with draw the world - robot = motion.robot - q = robot.getKlamptSensedPosition() - self.world.robot(0).setConfig(q) - self.world.robot(0).drawGL() - self.world.terrain(0).drawGL() - GLWidgetProgram.display(self) - - #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 keyboardfunc(self,c,x,y): - #Put your keyboard handler here - #the current example toggles simulation / movie mode - if c == 'h': - print '[space]: send the current posed milestone' - print 'q: clean quit' - elif c == ' ': - q = self.robotPoser.get() - q0 = motion.robot.getKlamptCommandedPosition() - t1 = time.time() - #collisions = obstaclecollision(WorldCollider(self.world),self.world.robot(0),q0,q) - collides = bisection(WorldCollider(self.world),self.world.robot(0),q0,q) - print "Obstacle collision detection done in time", + time.time()-t1 - exit(0) - for i in range(self.world.robot(0).numLinks()): - self.world.robot(0).link(i).appearance().setColor(0.5,0.5,0.5,1) - #if not self.firstTimeHack and selfcollision(self.world.robot(0),q0,q): - if collides: - print "Invalid configuration, it self-collides" - - elif not self.firstTimeHack and collisions!=None: - #clear all links to gray - for pairs in collisions: - print "Link "+str(pairs[0].getIndex())+" collides with obstacle" - self.world.robot(0).link(pairs[0].getIndex()).appearance().setColor(1,0,0,1) - else: - self.firstTimeHack = False - robot = motion.robot - robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q)) - robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q)) - qlg = robot.left_gripper.configFromKlampt(q) - qrg = robot.right_gripper.configFromKlampt(q) - robot.left_gripper.command(qlg,[1]*len(qlg),[1]*len(qlg)) - robot.right_gripper.command(qrg,[1]*len(qrg),[1]*len(qrg)) - #robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices]) - #robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices]) - elif c == 'q': - motion.robot.shutdown() - exit(0) - else: - GLWidgetProgram.keyboardfunc(self,c,x,y) - self.refresh() - - def subscribe(self): - rospy.init_node('subscribe', anonymous=True) - rospy.Subscriber("/kinect2/sd/points", PointCloud2, self.callback) - - def callback(self,data): - if self.newPC == None: - data_out = pc2.read_points(data,skip_nans=True) - #data_out = pc2.read_points(data) - pc = PointCloud() - pc.propertyNames.append('rgba') - t0 = time.time() - points = list(data_out) - - #filtered = self.radiusfilter(points,0.1,0) - filtered = self.voxelgridfilter(points, 0.1, 0.1, 0.1, 10) - print "Noise cleaned up in time", time.time()-t0 - #filtered = points - for p in filtered: - coordinates = (p[0],p[1],p[2]) - if vectorops.norm(coordinates)>0.4: - index = pc.addPoint(coordinates) - i = struct.unpack("i",struct.pack("f",p[3]))[0] - pc.setProperty(index, 0, i) - #print "Point cloud copied to Klampt format in time",time.time()-t0 - - q = (0.566222,0.423455,0.424871,0.5653) - R1 = so3.from_quaternion(q) - t1 = (0.228665,0.0591513,0.0977748) - T1 = (R1, t1) - R2 = so3.rotation((0,0,1),math.pi/2) - t2 = (0,0,1) - T2 = (R2, t2) - T = se3.mul(T2,T1) - (R,t)=T - pc.transform(R,t) - #print "Point cloud transformed in time",time.time()-t0 - self.newPC = pc - else: - - print "Not ready yet to receive new point cloud..." - - # Color data will be lost - def statisticalfilter(self,points,k,mul): - pclCloud = pcl.PointCloud() - pclCloud.from_list(points) - fil = pclCloud.make_statistical_outlier_filter() - fil.set_mean_k(k) - - fil.set_std_dev_mul_thresh(mul) - filtered = fil.filter().to_list() - return filtered - # Under construction - def radiusfilter(self,points,radius,num): - filtered = [] - coordinates = [] - for p in points: - coordinates.append([p[0],p[1],p[2]]) - #print np.array(coordinates).reshape(-len(coordinates),3) - tree = KDTree(np.array(coordinates)) - #tree = KDTree(np.random.random((10, 3)), leaf_size=2) - for i,p in enumerate(coordinates): - #print tree.query_radius(p,r=radius,count_only=True) - if tree.query_radius(coordinates[i],r=radius,count_only=True)>=num: - filtered.append(points[i]) - return points - - def voxelgridfilter(self,points,gridX,gridY,gridZ,num): - filtered = [] - dictionary = {} - for i, p in enumerate(points): - x = p[0] - y = p[1] - z = p[2] - xindex = int (math.floor(x/gridX)) - yindex = int (math.floor(y/gridY)) - zindex = int (math.floor(z/gridZ)) - key = (xindex,yindex,zindex) - if key not in dictionary: - dictionary[key]=[] - else: - dictionary[key].append(i) - for key,value in dictionary.iteritems(): - if len(value)>=num: - for j in value: - filtered.append(points[j]) - return filtered - - - #def countNeighbors(self, points, radius): - # dictionary = defaultdict(int) - # for i in range(len(points)-1): - # for j in range(i+1,len(points)): - # pointA = points[i] - # pointB = points[j] - # if(vectorops.distance((pointA[0],pointA[1],pointA[2]),(pointB[0],pointB[1],pointB[2]))= 6 - assert len(r) >= 6 - assert len(processed_data) >=12 - - #joint commands for right arm - q[0] = 0.00388 * processed_data[0] - 2.01026 #-1.7 + processed_data[0] * 3.4/1023 - q[1] = 0.00408 * processed_data[1] - 2.86122 #-2.15 + processed_data[1] * 3.2/1023 - q[2] = 0.00395 * processed_data[2] - 2.40745 #-3.1 + processed_data[2] * 6.1/1023 - q[3] = 0.00395 * processed_data[3] - 1.51872 #-0.05 + processed_data[3] * 2.65/1023 - q[4] = 0.00367 * processed_data[4] - 2.3802 #-3.05 + processed_data[4] * 6.1/1023 - q[5] = 0.003747 * processed_data[5] - 1.8035 #-1.6 + processed_data[5] * 3.7/1023 - #joint commands for left arm - r[0] = 0.00365 * processed_data[6] - 1.98829 #-1.7 + processed_data[6] * 3.4/1023 - r[1] = 0.003744 * processed_data[7] - 2.0879 #-2.15 + processed_data[7] * 3.2/1023 - r[2] = 0.003713 * processed_data[8] - 2.85335 #-3.1 + processed_data[8] * 6.1/1023 - r[3] = 0.00416 * processed_data[9] - 1.86292 #-0.05 + processed_data[9] * 2.65/1023 - r[4] = 0.003949 * processed_data[10] - 3.30957 #-3.05 + processed_data[10] * 6.1/1023 - r[5] = 0.00397 * processed_data[11] - 1.93171 #-1.6 + processed_data[11] * 3.7/1023 - - robot.left_limb.positionCommand(q) - robot.right_limb.positionCommand(r) - 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 "puppet_test.py: tests the RoboPuppet with the motion module" - print "Press q to exit." - print - - config.setup() - res = motion.robot.startup() - if not res: - print "Error starting up APC Motion" - exit(1) - time.sleep(0.1) - world = WorldModel() - res = world.readFile(config.klampt_model) - if not res: - raise RuntimeError("Unable to load APC Motion model "+fn) - - viewer = MyGLViewer(world) - viewer.run() diff --git a/common/Testing/simpletest.py b/common/Testing/simpletest.py index 75d9a4b..2a0bdf0 100644 --- a/common/Testing/simpletest.py +++ b/common/Testing/simpletest.py @@ -1,13 +1,19 @@ +import sys; sys.path.append('.') from Motion import motion -motion.setup(mode='client',klampt_model='klampt_models/baxter_with_parallel_grippers',libpath='.') +#manual motion setup +motion.setup(mode='kinematic',klampt_model='klampt_models/baxter_with_parallel_gripper_col.rob',libpath='./') +#automatic motion setup +#from Motion import config +#config.setup(parse_sys=False) motion.robot.startup() try: while True: - raw_input() + raw_input("Press enter to print sensed left arm positions > ") print motion.robot.left_limb.sensedPosition() except KeyboardInterrupt: - motion.robot.stopMotion() + print "Shutting down..." + diff --git a/common/Testing/watcher.py b/common/Testing/watcher.py new file mode 100644 index 0000000..9b5775a --- /dev/null +++ b/common/Testing/watcher.py @@ -0,0 +1,71 @@ +from klampt import * +from klampt.glprogram import * +import time +import sys +import os +ebolabot_root = os.getenv("EBOLABOT_PATH",".") +sys.path.append(ebolabot_root) +from Motion import motion +from Motion import config + + +class MyGLViewer(GLRealtimeProgram): + def __init__(self,world): + GLRealtimeProgram.__init__(self,"Viewer") + self.world = world + + robot = world.robot(0) + + def display(self): + #Put your display handler here + #the current example draws the sensed robot in grey and the + #commanded configurations in transparent green + + #these lines will update the robot and draw the world + robot = motion.robot + q = robot.getKlamptSensedPosition() + self.world.robot(0).setConfig(q) + GLWidgetProgram.display(self) + + #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 keyboardfunc(self,c,x,y): + #Put your keyboard handler here + #the current example toggles simulation / movie mode + if c == 'h': + print 'q: clean quit' + elif c == 'q': + motion.robot.shutdown() + exit(0) + else: + GLWidgetProgram.keyboardfunc(self,c,x,y) + self.refresh() + + +if __name__ == "__main__": + config.parse_args() + print "watcher.py: updates and visualizes the robot. Runs in client mode" + print + + print "Loading Motion Module model",config.klampt_model + config.mode = 'client' + motion.setup(mode=config.mode,klampt_model=config.klampt_model,libpath="./",) + res = motion.robot.startup() + if not res: + print "Error starting up Motion Module" + exit(1) + time.sleep(0.1) + world = WorldModel() + res = world.readFile(config.klampt_model) + if not res: + raise RuntimeError("Unable to load Klamp't model "+config.klampt_model) + + viewer = MyGLViewer(world) + viewer.run() diff --git a/common/Testing/widget_ui.py b/common/Testing/widget_ui.py deleted file mode 100644 index 3b51e95..0000000 --- a/common/Testing/widget_ui.py +++ /dev/null @@ -1,154 +0,0 @@ -from klampt import * -from klampt import WidgetSet,RobotPoser -from klampt.glprogram import * -from klampt.glcommon import GLWidgetPlugin -import math -import time -import signal -from sspp.topic import * -ebolabot_root = os.getenv("EBOLABOT_PATH",".") -sys.path.append(ebolabot_root) -from Common.system_config import EbolabotSystemConfig -import threading - -#Configuration variable: where's the state server? -system_state_addr = EbolabotSystemConfig.getdefault_ip('state_server_computer_ip',('localhost',4568)) - - - -class GLWidgetProgram(GLPluginProgram): - """A program that uses a widget plugin""" - def __init__(self,name): - GLPluginProgram.__init__(self,"Manual poser") - self.widgetPlugin = GLWidgetPlugin() - def initialize(self): - GLPluginProgram.initialize(self) - self.setPlugin(self.widgetPlugin) - def addWidget(self,widget): - self.widgetPlugin.addWidget(widget) - -class MyGLViewer(GLWidgetProgram): - def __init__(self,world,serviceThread): - GLWidgetProgram.__init__(self,"Manual poser") - - self.world = world - self.serviceThread = serviceThread - - for i in range(10): - q = self.serviceThread.qcmdGetter.get() - if q != None: - break - time.sleep(0.1) - if q == None: - print "Warning, system state service doesn't seem to have .robot.command.q" - print "Press enter to continue" - raw_input() - else: - world.robot(0).setConfig(q) - self.robotPoser = RobotPoser(world.robot(0)) - self.addWidget(self.robotPoser) - - robot = world.robot(0) - left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist'] - right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist'] - self.left_arm_link_indices = [robot.link(l).index for l in left_arm_link_names] - self.right_arm_link_indices = [robot.link(l).index for l in right_arm_link_names] - - def display(self): - #Put your display handler here - #the example draws the posed robot in grey, the sensed - #configuration in transparent red and the commanded - #configuration in transparent green - - #this line will draw the world - GLWidgetProgram.display(self) - - q = self.serviceThread.qsnsGetter.get() - if q: - glEnable(GL_BLEND) - glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA) - glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,0,0,0.5]) - self.world.robot(0).setConfig(q) - self.world.robot(0).drawGL(False) - - #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 = self.serviceThread.qcmdGetter.get() - if q: - self.world.robot(0).setConfig(q) - self.world.robot(0).drawGL(False) - glDisable(GL_BLEND) - - def keyboardfunc(self,c,x,y): - #Put your keyboard handler here - #the current example toggles simulation / movie mode - if c == 'h': - print '[space]: send the current posed milestone' - print 'q: clean quit' - elif c == ' ': - q = self.robotPoser.get() - #CONSTRUCT THE TASK HERE - msg = {} - msg['type'] = 'JointPose' - msg['limb'] = 'both' - msg['position'] = [q[i] for i in self.left_arm_link_indices + self.right_arm_link_indices] - msg['speed'] = 1 - msg['safe'] = 0 - print "Sending message",msg - #SEND THE TASK TO THE SYSTEM STATE SERVICE - self.serviceThread.taskSetter.set(msg) - elif c == 'q': - self.serviceThread.kill() - exit(0) - else: - GLWidgetProgram.keyboardfunc(self,c,x,y) - self.refresh() - - -class ServiceThread(threading.Thread): - """This thread takes care of running asyncore while the GUI is running""" - def __init__(self): - threading.Thread.__init__(self) - self._kill = False - self.updateFreq = 100 - #set up the listeners: sensed, commanded, and destination configs - self.qsnsGetter = TopicListener(system_state_addr,'.robot.sensed.q',20) - self.qcmdGetter = TopicListener(system_state_addr,'.robot.command.q',20) - self.qdstGetter = TopicListener(system_state_addr,'.controller.traj_q_end',20) - self.statusGetter = TopicListener(system_state_addr,'.controller.task_status',20) - self.taskSetter = TopicServiceBase('.controller.task') - self.taskSetter.open(system_state_addr) - def run(self): - while(not self._kill): - asyncore.loop(timeout = 1.0/self.updateFreq, count=1) - def kill(self): - self._kill = True - -if __name__ == "__main__": - - print "widget_ui.py: manually sends configurations to the ControllerDispatcher" - print "Press [space] to send milestones. Press q to exit." - print - klampt_model = EbolabotSystemConfig.get("klampt_model",lambda s:s.encode('ascii','ignore')) - world = WorldModel() - res = world.readFile(klampt_model) - if not res: - raise RuntimeError("Unable to load Ebolabot model "+klampt_model) - - serviceThread = ServiceThread() - serviceThread.start() - - #cleanly handle Ctrl+C - def handler(signal,frame): - print "Exiting due to Ctrl+C" - serviceThread.kill() - exit(0) - signal.signal(signal.SIGINT,handler) - - viewer = MyGLViewer(world,serviceThread) - viewer.run() - - #cleanup - serviceThread.kill() diff --git a/common/libmotion_kinematic.so b/common/libmotion_kinematic.so index 2e9d305..86598de 100755 Binary files a/common/libmotion_kinematic.so and b/common/libmotion_kinematic.so differ diff --git a/common/libmotion_physical.so b/common/libmotion_physical.so index c826229..6a76c91 100755 Binary files a/common/libmotion_physical.so and b/common/libmotion_physical.so differ