added move_robot.py

2018-05-27 16:35:17 +02:00
parent 4d78c5c85c
commit 2033a69fdb
1 changed files with 150 additions and 0 deletions
--- a/scripts/move_robot.py
+++ b/scripts/move_robot.py
@@ -0,0 +1,150 @@
+# -*- encoding: UTF-8 -*-
+
+''' PoseInit: Small example to make Nao go to an initial position. '''
+
+import argparse
+from naoqi import ALProxy
+import time
+import math
+import almath
+#include <alproxies/alnavigationproxy.h>
+
+
+def main(robotIP, PORT=9559):
+
+    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
+    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
+
+    # Wake up robot
+    motionProxy.wakeUp()
+    
+    # set the stiffnes
+    motionProxy.setStiffnesses("Head", 1.0)
+    motionProxy.setStiffnesses("RHand", 1.0)	
+    motionProxy.setStiffnesses("LHand", 1.0)
+    motionProxy.setStiffnesses("LShoulderPitch", 1.0)
+    motionProxy.setStiffnesses("LShoulderRoll", 1.0)
+    motionProxy.setStiffnesses("LElbowRoll", 1.0)
+    motionProxy.setStiffnesses("LWristYaw", 1.0)
+    motionProxy.setStiffnesses("LElbowYaw", 1.0)
+    motionProxy.setStiffnesses("LHipYawPitch", 1.0)
+    motionProxy.setStiffnesses("LHipPitch", 1.0)
+    motionProxy.setStiffnesses("LKneePitch", 1.0)
+    motionProxy.setStiffnesses("LAnklePitch", 1.0)
+    motionProxy.setStiffnesses("LHipRoll", 1.0)
+    motionProxy.setStiffnesses("LAnkleRoll", 1.0)
+    
+    motionProxy.setStiffnesses("RShoulderPitch", 1.0)
+    motionProxy.setStiffnesses("RShoulderRoll", 1.0)
+    motionProxy.setStiffnesses("RElbowRoll", 1.0)
+    motionProxy.setStiffnesses("RWristYaw", 1.0)
+    motionProxy.setStiffnesses("RElbowYaw", 1.0)
+    motionProxy.setStiffnesses("RHipYawPitch", 1.0)
+    motionProxy.setStiffnesses("RHipPitch", 1.0)
+    motionProxy.setStiffnesses("RKneePitch", 1.0)
+    motionProxy.setStiffnesses("RAnklePitch", 1.0)
+    motionProxy.setStiffnesses("RHipRoll", 1.0)
+    motionProxy.setStiffnesses("RAnkleRoll", 1.0)
+
+    # Send robot to Stand Init
+    postureProxy.goToPosture("StandInit", 0.5)
+
+    # Initialize the move
+    motionProxy.moveInit()
+
+    # First call of move API
+    # with post prefix to not be bloquing here.
+    motionProxy.post.moveTo(-0.3, 0.0, 0)
+    
+    # wait that the move process start running
+    time.sleep(0.1)
+
+    # 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)
+
+    # 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)
+
+    # 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
+
+
+    # Go to rest position
+    motionProxy.rest()
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--ip", type=str, default="192.168.0.11",
+                        help="Robot ip address")
+    parser.add_argument("--port", type=int, default=9559,
+                        help="Robot port number")
+
+    args = parser.parse_args()
+    main(args.ip, args.port)