modified move_robot.py

scripts/move_robot.py

@@ -18,33 +18,47 @@ def main(robotIP, PORT=9559):
     # Wake up robot
     motionProxy.wakeUp()
     
+    handStiffness=0.0;
+    # shoulder and elbow
+    armStiffness = 0.0;
+    # head
+    headStiffness = 0.9;
+    # hip
+    hipStiffness = 0.9;
+    # knee
+    kneeStiffness = 0.9;
+    # ankle
+    ankleStiffness = 0.9;
+
+
     # set the stiffnes
-    motionProxy.setStiffnesses("Head", 1.0)
+    motionProxy.setStiffnesses("Head", headStiffness)
-    motionProxy.setStiffnesses("RHand", 1.0)	
+    motionProxy.setStiffnesses("RHand", handStiffness)
-    motionProxy.setStiffnesses("LHand", 1.0)
+    motionProxy.setStiffnesses("LHand", handStiffness)
-    motionProxy.setStiffnesses("LShoulderPitch", 1.0)
+    motionProxy.setStiffnesses("LShoulderPitch", armStiffness)
-    motionProxy.setStiffnesses("LShoulderRoll", 1.0)
+    motionProxy.setStiffnesses("LShoulderRoll", armStiffness)
-    motionProxy.setStiffnesses("LElbowRoll", 1.0)
+    motionProxy.setStiffnesses("LElbowRoll", armStiffness)
-    motionProxy.setStiffnesses("LWristYaw", 1.0)
+    motionProxy.setStiffnesses("LWristYaw", handStiffness)
-    motionProxy.setStiffnesses("LElbowYaw", 1.0)
+    motionProxy.setStiffnesses("LElbowYaw", armStiffness)
-    motionProxy.setStiffnesses("LHipYawPitch", 1.0)
+    motionProxy.setStiffnesses("LHipYawPitch", hipStiffness)
-    motionProxy.setStiffnesses("LHipPitch", 1.0)
+    motionProxy.setStiffnesses("LHipPitch", hipStiffness)
-    motionProxy.setStiffnesses("LKneePitch", 1.0)
+    motionProxy.setStiffnesses("LKneePitch", kneeStiffness)
-    motionProxy.setStiffnesses("LAnklePitch", 1.0)
+    motionProxy.setStiffnesses("LAnklePitch", ankleStiffness)
-    motionProxy.setStiffnesses("LHipRoll", 1.0)
+    motionProxy.setStiffnesses("LHipRoll", hipStiffness)
-    motionProxy.setStiffnesses("LAnkleRoll", 1.0)
+    motionProxy.setStiffnesses("LAnkleRoll", ankleStiffness)
-    
+
-    motionProxy.setStiffnesses("RShoulderPitch", 1.0)
+    motionProxy.setStiffnesses("RShoulderPitch", armStiffness)
-    motionProxy.setStiffnesses("RShoulderRoll", 1.0)
+    motionProxy.setStiffnesses("RShoulderRoll", armStiffness)
-    motionProxy.setStiffnesses("RElbowRoll", 1.0)
+    motionProxy.setStiffnesses("RElbowRoll", armStiffness)
-    motionProxy.setStiffnesses("RWristYaw", 1.0)
+    motionProxy.setStiffnesses("RWristYaw", handStiffness)
-    motionProxy.setStiffnesses("RElbowYaw", 1.0)
+    motionProxy.setStiffnesses("RElbowYaw", armStiffness)
-    motionProxy.setStiffnesses("RHipYawPitch", 1.0)
+    motionProxy.setStiffnesses("RHipYawPitch", hipStiffness)
-    motionProxy.setStiffnesses("RHipPitch", 1.0)
+    motionProxy.setStiffnesses("RHipPitch", hipStiffness)
-    motionProxy.setStiffnesses("RKneePitch", 1.0)
+    motionProxy.setStiffnesses("RKneePitch", kneeStiffness)
-    motionProxy.setStiffnesses("RAnklePitch", 1.0)
+    motionProxy.setStiffnesses("RAnklePitch", ankleStiffness)
-    motionProxy.setStiffnesses("RHipRoll", 1.0)
+    motionProxy.setStiffnesses("RHipRoll", hipStiffness)
-    motionProxy.setStiffnesses("RAnkleRoll", 1.0)
+    motionProxy.setStiffnesses("RAnkleRoll", ankleStiffness)
+
 
     # Send robot to Stand Init
     postureProxy.goToPosture("StandInit", 0.5)
@@ -54,7 +68,7 @@ def main(robotIP, PORT=9559):
 
     # First call of move API
     # with post prefix to not be bloquing here.
-    motionProxy.post.moveTo(-0.3, 0.0, 0)
+    motionProxy.post.moveTo(0, 0.0, 0)
     
     # wait that the move process start running
     time.sleep(0.1)
@@ -74,70 +88,47 @@ def main(robotIP, PORT=9559):
         print str(errorMsg)
         PLOT_ALLOW = False
 
-    # Second call of move API
+    while True:
-    motionProxy.post.moveTo(0.3, 0.0, 0)
+        cmd = int(raw_input(">"))
-
-    # get the second foot steps vector
-    footSteps2 = []
-    try:
-        footSteps2 = motionProxy.getFootSteps()
-    except Exception, errorMsg:
-        print str(errorMsg)
-        PLOT_ALLOW = False
-
-    # end experiment, begin compute
-
-    # here we wait until the move process is over
-    motionProxy.waitUntilMoveIsFinished()
-    # then we get the final robot position
-    robotPositionFinal = almath.Pose2D(motionProxy.getRobotPosition(False))
-
-    # compute robot Move with the second call of move API
-    # so between nextRobotPosition and robotPositionFinal
-    robotMove = almath.pose2DInverse(nextRobotPosition)*robotPositionFinal
-    print "Robot Move:", robotMove
-
-     # get robotPosition and nextRobotPosition
-    useSensors = False
-    robotPosition     = almath.Pose2D(motionProxy.getRobotPosition(useSensors))
-    nextRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())
-
-    # get the first foot steps vector
-    # (footPosition, unChangeable and changeable steps)
-
-    footSteps1 = []
-    try:
-        footSteps1 = motionProxy.getFootSteps()
-    except Exception, errorMsg:
-        print str(errorMsg)
-        PLOT_ALLOW = False
 
     # Second call of move API
-    motionProxy.post.moveTo(-0.3, 0.0, 0)
+        motionProxy.post.moveTo(3, 0, 0)
-
-    # get the second foot steps vector
-    footSteps2 = []
-    try:
-        footSteps2 = motionProxy.getFootSteps()
-    except Exception, errorMsg:
-        print str(errorMsg)
-        PLOT_ALLOW = False
-
-    # end experiment, begin compute
 
     # here we wait until the move process is over
-    motionProxy.waitUntilMoveIsFinished()
+        motionProxy.waitUntilMoveIsFinished()
-    # then we get the final robot position
+   
-    robotPositionFinal = almath.Pose2D(motionProxy.getRobotPosition(False))
+    ''' 
+     # Second call of move API
+    motionProxy.post.moveTo(1.5, 0, 0)
+
+     # here we wait until the move process is over
+    motionProxy.waitUntilMoveIsFinished()
+
+   
+     # Second call of move API
+    motionProxy.post.moveTo(0, -1.5, 0)
+   
+     # here we wait until the move process is over
+    motionProxy.waitUntilMoveIsFinished()
+
+      # Second call of move API
+    motionProxy.post.moveTo(-1.5, 0, 0)
+
+     # here we wait until the move process is over
+    motionProxy.waitUntilMoveIsFinished()
+    '''
+
+      # Second call of move API
+#    motionProxy.post.moveTo(0, 3, 0)
+
+     # here we wait until the move process is over
+ #   motionProxy.waitUntilMoveIsFinished()
+
 
-    # compute robot Move with the second call of move API
-    # so between nextRobotPosition and robotPositionFinal
-    robotMove = almath.pose2DInverse(nextRobotPosition)*robotPositionFinal
-    print "Robot Move:", robotMove
 
 
     # Go to rest position
-    motionProxy.rest()
+    #motionProxy.rest()
 
 if __name__ == "__main__":
     parser = argparse.ArgumentParser()