modified move_robot.py
This commit is contained in:
Jonas
@@ -18,33 +18,47 @@ def main(robotIP, PORT=9559):
|
|||||||
# Wake up robot
|
# Wake up robot
|
||||||
motionProxy.wakeUp()
|
motionProxy.wakeUp()
|
||||||
|
|
||||||
# set the stiffnes
|
handStiffness=0.0;
|
||||||
motionProxy.setStiffnesses("Head", 1.0)
|
# shoulder and elbow
|
||||||
motionProxy.setStiffnesses("RHand", 1.0)
|
armStiffness = 0.0;
|
||||||
motionProxy.setStiffnesses("LHand", 1.0)
|
# head
|
||||||
motionProxy.setStiffnesses("LShoulderPitch", 1.0)
|
headStiffness = 0.9;
|
||||||
motionProxy.setStiffnesses("LShoulderRoll", 1.0)
|
# hip
|
||||||
motionProxy.setStiffnesses("LElbowRoll", 1.0)
|
hipStiffness = 0.9;
|
||||||
motionProxy.setStiffnesses("LWristYaw", 1.0)
|
# knee
|
||||||
motionProxy.setStiffnesses("LElbowYaw", 1.0)
|
kneeStiffness = 0.9;
|
||||||
motionProxy.setStiffnesses("LHipYawPitch", 1.0)
|
# ankle
|
||||||
motionProxy.setStiffnesses("LHipPitch", 1.0)
|
ankleStiffness = 0.9;
|
||||||
motionProxy.setStiffnesses("LKneePitch", 1.0)
|
|
||||||
motionProxy.setStiffnesses("LAnklePitch", 1.0)
|
|
||||||
motionProxy.setStiffnesses("LHipRoll", 1.0)
|
# set the stiffnes
|
||||||
motionProxy.setStiffnesses("LAnkleRoll", 1.0)
|
motionProxy.setStiffnesses("Head", headStiffness)
|
||||||
|
motionProxy.setStiffnesses("RHand", handStiffness)
|
||||||
|
motionProxy.setStiffnesses("LHand", handStiffness)
|
||||||
|
motionProxy.setStiffnesses("LShoulderPitch", armStiffness)
|
||||||
|
motionProxy.setStiffnesses("LShoulderRoll", armStiffness)
|
||||||
|
motionProxy.setStiffnesses("LElbowRoll", armStiffness)
|
||||||
|
motionProxy.setStiffnesses("LWristYaw", handStiffness)
|
||||||
|
motionProxy.setStiffnesses("LElbowYaw", armStiffness)
|
||||||
|
motionProxy.setStiffnesses("LHipYawPitch", hipStiffness)
|
||||||
|
motionProxy.setStiffnesses("LHipPitch", hipStiffness)
|
||||||
|
motionProxy.setStiffnesses("LKneePitch", kneeStiffness)
|
||||||
|
motionProxy.setStiffnesses("LAnklePitch", ankleStiffness)
|
||||||
|
motionProxy.setStiffnesses("LHipRoll", hipStiffness)
|
||||||
|
motionProxy.setStiffnesses("LAnkleRoll", ankleStiffness)
|
||||||
|
|
||||||
|
motionProxy.setStiffnesses("RShoulderPitch", armStiffness)
|
||||||
|
motionProxy.setStiffnesses("RShoulderRoll", armStiffness)
|
||||||
|
motionProxy.setStiffnesses("RElbowRoll", armStiffness)
|
||||||
|
motionProxy.setStiffnesses("RWristYaw", handStiffness)
|
||||||
|
motionProxy.setStiffnesses("RElbowYaw", armStiffness)
|
||||||
|
motionProxy.setStiffnesses("RHipYawPitch", hipStiffness)
|
||||||
|
motionProxy.setStiffnesses("RHipPitch", hipStiffness)
|
||||||
|
motionProxy.setStiffnesses("RKneePitch", kneeStiffness)
|
||||||
|
motionProxy.setStiffnesses("RAnklePitch", ankleStiffness)
|
||||||
|
motionProxy.setStiffnesses("RHipRoll", hipStiffness)
|
||||||
|
motionProxy.setStiffnesses("RAnkleRoll", ankleStiffness)
|
||||||
|
|
||||||
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
|
# Send robot to Stand Init
|
||||||
postureProxy.goToPosture("StandInit", 0.5)
|
postureProxy.goToPosture("StandInit", 0.5)
|
||||||
@@ -54,7 +68,7 @@ def main(robotIP, PORT=9559):
|
|||||||
|
|
||||||
# First call of move API
|
# First call of move API
|
||||||
# with post prefix to not be bloquing here.
|
# 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
|
# wait that the move process start running
|
||||||
time.sleep(0.1)
|
time.sleep(0.1)
|
||||||
@@ -74,70 +88,47 @@ def main(robotIP, PORT=9559):
|
|||||||
print str(errorMsg)
|
print str(errorMsg)
|
||||||
PLOT_ALLOW = False
|
PLOT_ALLOW = False
|
||||||
|
|
||||||
|
while True:
|
||||||
|
cmd = int(raw_input(">"))
|
||||||
|
|
||||||
# Second call of move API
|
# 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
|
# 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))
|
|
||||||
|
|
||||||
# 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
|
# Second call of move API
|
||||||
motionProxy.post.moveTo(-0.3, 0.0, 0)
|
motionProxy.post.moveTo(1.5, 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
|
# 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))
|
|
||||||
|
|
||||||
# compute robot Move with the second call of move API
|
|
||||||
# so between nextRobotPosition and robotPositionFinal
|
# Second call of move API
|
||||||
robotMove = almath.pose2DInverse(nextRobotPosition)*robotPositionFinal
|
motionProxy.post.moveTo(0, -1.5, 0)
|
||||||
print "Robot Move:", robotMove
|
|
||||||
|
# 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()
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
# Go to rest position
|
# Go to rest position
|
||||||
motionProxy.rest()
|
#motionProxy.rest()
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
parser = argparse.ArgumentParser()
|
parser = argparse.ArgumentParser()
|
||||||
|
|||||||
Reference in New Issue
Block a user