modified move_robot.py
This commit is contained in:
Jonas
@@ -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("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)
|
||||
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)
|
||||
|
||||
|
||||
# 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
|
||||
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
|
||||
while True:
|
||||
cmd = int(raw_input(">"))
|
||||
|
||||
# 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
|
||||
motionProxy.post.moveTo(3, 0, 0)
|
||||
|
||||
# here we wait until the move process is over
|
||||
motionProxy.waitUntilMoveIsFinished()
|
||||
# then we get the final robot position
|
||||
robotPositionFinal = almath.Pose2D(motionProxy.getRobotPosition(False))
|
||||
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, -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()
|
||||
|
||||
Reference in New Issue
Block a user