update the common folder

common/Motion/config.py

@@ -4,8 +4,8 @@
 #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"

common/Motion/motion_physical.cpp

@@ -608,7 +608,7 @@ void MyControllerUpdateData::MySendCommands()
     ScopedLock lock(mutex);
     baxter_core_msgs::HeadPanCommand msg;
     msg.target = robotState.head.panTarget;
-    msg.speed_ratio = robotState.head.panSpeed;
+    msg.speed = robotState.head.panSpeed;
     head_pan_pub.publish(msg);
     robotState.head.sendCommand = false;
   }

common/Testing/GamePad_noGUI.py

@@ -0,0 +1,103 @@
+"""
+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)

common/Testing/base_calibrate.py

@@ -0,0 +1,125 @@
+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."