diff --git a/README.md b/README.md index 07862fc..979c414 100644 --- a/README.md +++ b/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. diff --git a/common/Motion/motion.h b/common/Motion/motion.h index 19251de..b4e7321 100644 --- a/common/Motion/motion.h +++ b/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); diff --git a/common/Motion/motion.py b/common/Motion/motion.py index e4f70aa..20cd959 100644 --- a/common/Motion/motion.py +++ b/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' diff --git a/common/Motion/motion_common.h b/common/Motion/motion_common.h index 0967376..2200f96 100644 --- a/common/Motion/motion_common.h +++ b/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 /////// diff --git a/common/Motion/motion_kinematic.cpp b/common/Motion/motion_kinematic.cpp index 662194b..16881c4 100644 --- a/common/Motion/motion_kinematic.cpp +++ b/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; diff --git a/common/Testing/simpletest.py b/common/Testing/simpletest.py new file mode 100644 index 0000000..75d9a4b --- /dev/null +++ b/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() + + diff --git a/common/Testing/widget_control.py b/common/Testing/widget_control.py index f692765..d141dcb 100644 --- a/common/Testing/widget_control.py +++ b/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)