def adjust_front(): motionProxy = config.loadProxy("ALMotion") config.StiffnessOn(motionProxy) x = 0.05 y = 0.0 theta = 0.0 motionProxy.walkTo(x, y, theta, [ ["StepHeight", 0.02] ]) # step height of 4 cm
def main(): ''' Example showing a hand ellipsoid Warning: Needs a PoseInit before executing Example available: path/to/aldebaran-sdk/modules/src/examples/ python/motion_cartesianArm2.py ''' proxy = config.loadProxy("ALMotion") #Set NAO in stiffness On config.StiffnessOn(proxy) # send robot to Pose Init config.PoseInit(proxy) effector = "LArm" space = motion.SPACE_NAO path = [ [0.0, -0.02, +0.00, 0.0, 0.0, 0.0], # Point 1 [0.0, +0.00, +0.01, 0.0, 0.0, 0.0], # Point 2 [0.0, +0.08, +0.00, 0.0, 0.0, 0.0], # Point 3 [0.0, +0.00, -0.04, 0.0, 0.0, 0.0], # Point 4 [0.0, -0.02, +0.00, 0.0, 0.0, 0.0], # Point 5 [0.0, +0.00, +0.00, 0.0, 0.0, 0.0] ] # Point 6 axisMask = 7 # just control position times = [0.5, 1.0, 2.0, 3.0, 4.0, 4.5] # seconds isAbsolute = False proxy.positionInterpolation(effector, space, path, axisMask, times, isAbsolute)
def move(x, y, angle): ## flag=0 left flag=1 right motionProxy = config.loadProxy("ALMotion") #Set NAO in stiffness On config.StiffnessOn(motionProxy) config.PoseInit(motionProxy) ##################### ## Enable arms control by Walk algorithm ##################### motionProxy.setWalkArmsEnabled(True, True) #~ motionProxy.setWalkArmsEnabled(False, False) ##################### ## FOOT CONTACT PROTECTION ##################### #~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", False]]) motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]]) X = x * 0.01 Y = y * 0.01 Theta = angle motionProxy.post.walkTo(X, Y, Theta) # wait is useful because with post walkTo is not blocking function motionProxy.waitUntilWalkIsFinished()
def main(): ''' Example of a cartesian foot trajectory Warning: Needs a PoseInit before executing Example available: path/to/aldebaran-sdk/modules/src/examples/ python/motion_cartesianFoot.py ''' proxy = config.loadProxy("ALMotion") #Set NAO in stiffness On config.StiffnessOn(proxy) # send robot to Pose Init config.PoseInit(proxy) space = motion.SPACE_NAO axisMask = 63 # control all the effector's axes isAbsolute = False # Lower the Torso and move to the side effector = "Torso" path = [0.0, -0.07, -0.03, 0.0, 0.0, 0.0] time = 2.0 # seconds proxy.positionInterpolation(effector, space, path, axisMask, time, isAbsolute) # LLeg motion effector = "LLeg" path = [0.0, 0.06, 0.00, 0.0, 0.0, 0.8] times = 2.0 # seconds proxy.positionInterpolation(effector, space, path, axisMask, times, isAbsolute)
def main(): ''' Simultaneously control three effectors : the Torso, the Left Arm and the Right Arm Warning: Needs a PoseInit before executing Example available: path/to/aldebaran-sdk/modules/src/examples/ python/motion_cartesianTorsoArm1.py ''' proxy = config.loadProxy("ALMotion") #Set NAO in stiffness On config.StiffnessOn(proxy) # send robot to Pose Init config.PoseInit(proxy) space = motion.SPACE_NAO coef = 0.5 # motion speed times = [coef, 2.0 * coef, 3.0 * coef, 4.0 * coef] isAbsolute = False # Relative movement between current and desired positions dy = +0.06 # translation axis Y (meters) dz = -0.03 # translation axis Z (meters) dwx = +0.30 # rotation axis X (radians) # Motion of Torso with post process effector = "Torso" path = [ [0.0, -dy, dz, -dwx, 0.0, 0.0], # Point 1 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], # Point 2 [0.0, +dy, dz, +dwx, 0.0, 0.0], # Point 3 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] ] # Point 4 axisMask = 63 # control all the effector axes proxy.post.positionInterpolation(effector, space, path, axisMask, times, isAbsolute) # Motion of Arms with block process axisMask = 7 # control just the position times = [1.0 * coef, 2.0 * coef] # seconds # Motion of Right Arm during the first half of the Torso motion effector = "RArm" path = [ [0.0, -dy, 0.0, 0.0, 0.0, 0.0], # Point 1 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] ] # Point 2 proxy.positionInterpolation(effector, space, path, axisMask, times, isAbsolute) # Motion of Left Arm during the last half of the Torso motion effector = "LArm" path = [ [0.0, dy, 0.0, 0.0, 0.0, 0.0], # Point 1 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] ] # Point 2 proxy.positionInterpolation(effector, space, path, axisMask, times, isAbsolute)
def main(pChainName = "LArm"): ''' Example showing the effect of collision detection Nao bumps his chest with his left arm with collision detection enabled or disabled. ''' ################## # Initialization # ################## motionProxy = config.loadProxy("ALMotion") # Set NAO in stiffness On. config.StiffnessOn(motionProxy) # Send robot to Pose Init. config.poseZero(motionProxy) # Get the robot configuration. robotConfig = motionProxy.getRobotConfig() robotName = "" for i in range(len(robotConfig[0])): if (robotConfig[0][i] == "Model Type"): robotName = robotConfig[1][i] ############################### # Arm motion bumping on torso # ############################### # Disable collision detection on chainName. pEnable = True success = motionProxy.setCollisionProtectionEnabled(pChainName, pEnable) if (not success): print("Failed to disable collision protection") time.sleep(1.0) config.poseZero(motionProxy) moveHead.moveHead(motionProxy, "HeadYaw", 45) moveHead.moveHead(motionProxy, "HeadPitch", 45) moveHand.moveHand(motionProxy, "RHand", 45) moveLegs.moveLegs(motionProxy, "RHipYawPitch", 45) moveLegs.moveLegs(motionProxy, "LHipYawPitch", 45) moveFoot.moveFoot(motionProxy, "RAnklePitch", 45) time.sleep(3.0) # Make NAO's arm move so that it bumps its torso. #moveArm(motionProxy, robotName, pChainName, [0, 40, 0, 0]) #moveArm(motionProxy, robotName, "LArm", [0, 40, 0, 0]) #time.sleep(2.0) i = 0 # while i < 3: # moveArm(motionProxy, robotName, pChainName, [10, 0, -90, -50], 0.3) # moveArm(motionProxy, robotName, "LArm", [10, 10, -90, -50], 0.3) # time.sleep(1.0) # moveArm(motionProxy, robotName, pChainName, [10, -10, -40, -50], 0.3) # moveArm(motionProxy, robotName, "LArm", [10, -10, -40, -50], 0.3) # time.sleep(1.0) # i += 1 # Go back to pose init. config.poseZero(motionProxy)
def take_position(): motionProxy = config.loadProxy("ALMotion") config.StiffnessOn(motionProxy) D = find_goal.findgoal() theta = math.atan(D / 120.0) x = 0.30 * (1 - math.cos(theta)) y = -0.30 * (math.sin(theta)) print x, y, theta if D > 30.0 or D < -(30.0): motionProxy.walkTo(x, y, theta)
def main(): ''' Move the torso and keep arms fixed in nao space Warning: Needs a PoseInit before executing Example available: path/to/aldebaran-sdk/modules/src/examples/ python/motion_cartesianTorsoArm2.py ''' proxy = config.loadProxy("ALMotion") #Set NAO in stiffness On config.StiffnessOn(proxy) # send robot to Pose Init config.PoseInit(proxy) space = motion.SPACE_NAO coef = 1.0 # Speed motion times = [coef, 2.0*coef, 3.0*coef, 4.0*coef] # seconds isAbsolute = False dx = 0.04 # translation axis X (meters) dy = 0.04 # translation axis Y (meters) # Motion of the Torso effector = "Torso" path = [ [ 0.0, +dy, 0.0, 0.0, 0.0, 0.0], # Point 1 [ -dx, 0.0, 0.0, 0.0, 0.0, 0.0], # Point 2 [ 0.0, -dy, 0.0, 0.0, 0.0, 0.0], # Point 3 [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] # Point 4 axisMask = 63 # control all the effector axis # Using 'post' makes this command execute in a another thread proxy.post.positionInterpolation(effector, space, path, axisMask, times, isAbsolute) # Motion of Arms axisMask = 7 # control the position time = 4.0*coef # seconds # Arms have a null relative motion during all the torso motion path = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Point 1 # Motion of Right Arm effector = "RArm" proxy.post.positionInterpolation(effector, space, path, axisMask, time, isAbsolute) # Motion of Left Arm effector = "LArm" proxy.positionInterpolation(effector, space, path, axisMask, time, isAbsolute)
def headcheck(pitch,yaw): ''' Example of a whole body head orientation control Warning: Needs a PoseInit before executing Whole body balancer must be inactivated at the end of the script ''' motionProxy = config.loadProxy("ALMotion") # Set NAO in Stiffness On config.StiffnessOn(motionProxy) # Send NAO to Pose Init config.PoseInit(motionProxy) effectorName = "Head" # Active Head tracking isEnabled = True motionProxy.wbEnableEffectorControl(effectorName, isEnabled) # Example showing how to set orientation target for Head tracking # The 3 coordinates are absolute head orientation in NAO_SPACE # Rotation in RAD in x, y and z axis # X Axis Head Orientation feasible movement = [-20.0, +20.0] degree # Y Axis Head Orientation feasible movement = [-75.0, +70.0] degree # Z Axis Head Orientation feasible movement = [-30.0, +30.0] degree targetCoordinate=[0,pitch,yaw] # wbSetEffectorControl is a non blocking function # time.sleep allow head go to his target # The minimum period advised between two successives set commands is 0.2 s targetCoordinate = [target*math.pi/180.0 for target in targetCoordinate] motionProxy.wbSetEffectorControl(effectorName, targetCoordinate) time.sleep(3.0) # Deactivate Head tracking isEnabled = False motionProxy.wbEnableEffectorControl(effectorName, isEnabled)
def main(): ''' Example showing a path of five positions Warning: Needs a PoseInit before execution Example available: path/to/aldebaran-sdk/modules/src/examples/ python/motion_cartesianTorso.py ''' proxy = config.loadProxy("ALMotion") #Set NAO in stiffness On config.StiffnessOn(proxy) # send robot to Pose Init config.PoseInit(proxy) effector = "Torso" space = motion.SPACE_NAO # Position Only axisMask = 63 # Full control isAbsolute = False # define the changes relative to the current position dx = 0.040 # translation axis X (meter) dy = 0.040 # translation axis Y (meter) path = [ [+dx, 0.0, 0.0, 0.0, 0.0, 0.0], # Point 1 [0.0, -dy, 0.0, 0.0, 0.0, 0.0], # Point 2 [-dx, 0.0, 0.0, 0.0, 0.0, 0.0], # Point 3 [0.0, +dy, 0.0, 0.0, 0.0, 0.0], # Point 4 [+dx, 0.0, 0.0, 0.0, 0.0, 0.0], # Point 5 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] # Point 6 times = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # seconds proxy.positionInterpolation(effector, space, path, axisMask, times, isAbsolute)
def main(): ''' Example showing a path of two positions Warning: Needs a PoseInit before executing Example available: path/to/aldebaran-sdk/modules/src/examples/ python/motion_cartesianArm1.py ''' proxy = config.loadProxy("ALMotion") #Set NAO in stiffness On config.StiffnessOn(proxy) # send robot to Pose Init config.PoseInit(proxy) effector = "LArm" space = motion.SPACE_NAO axisMask = 7 # just control position isAbsolute = False # Since we are in relative, the current position is zero currentPos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # define the changes relative to the current position dx = 0.06 # translation axis X (meters) dy = 0.06 # translation axis Y (meters) dz = 0.00 # translation axis Z (meters) dwx = 0.00 # rotation axis X (radians) dwy = 0.00 # rotation axis Y (radians) dwz = 0.00 # rotation axis Z (radians) targetPos = [dx, dy, dz, dwx, dwy, dwz] # go to the target and back again path = [targetPos, currentPos] times = [2.0, 4.0] # seconds proxy.positionInterpolation(effector, space, path, axisMask, times, isAbsolute)
""" Step To Small example to make Nao one Step """ import time import math import config motionProxy = config.loadProxy("ALMotion") #Set NAO in stiffness On config.StiffnessOn(motionProxy) #~ DUBINS_INTERPOLATION = True DUBINS_INTERPOLATION = False ##################### ## Enable arms control by Walk algorithm ##################### motionProxy.setWalkArmsEnable(True, True) #~ motionProxy.setWalkArmsEnable(False, False) ##################### ## FOOT CONTACT PROTECTION ##################### #~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]]) motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]]) ##################### ##get robot position before move
import config print "Creating motion proxy" try: motion = config.loadProxy("ALMotion") config.StiffnessOn(motion) config.PoseInit(motion) except Exception, e: print "Error when creating motion proxy:" print str(e) exit(1) print "Creating ALVideoDevice proxy" try: cam = config.loadProxy("ALVideoDevice") except Exception, e: print "Error when creating vision proxy:" print str(e) exit(1)
def grab(): ''' Example of a whole body Enable Balance Constraint Warning: Needs a PoseInit before executing Whole body balancer must be inactivated at the end of the script ''' motionProxy = config.loadProxy("ALMotion") # Set NAO in Stiffness On config.StiffnessOn(motionProxy) # Activate Whole Body Balancer isEnabled = True motionProxy.wbEnable(isEnabled) names = [ "RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand", "LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw", "LHand", "RHipPitch", "RKneePitch", "RAnklePitch", "RAnkleRoll", "RHipRoll", "RHipYawPitch", "LHipPitch", "LKneePitch", "LAnklePitch", "LAnkleRoll", "LHipRoll", "LHipYawPitch" ] angleLists = [[ 80.0 * math.pi / 180.0, -10.0 * math.pi / 180.0, -14.0 * math.pi / 180.0, -14.0 * math.pi / 180.0, -14.0 * math.pi / 180.0 ], [ -80.0 * math.pi / 180.0, -80.0 * math.pi / 180.0, 0.0 * math.pi / 180.0, 0.0 * math.pi / 180.0, 0.0 * math.pi / 180.0 ], [ 80.0 * math.pi / 180.0, 50.0 * math.pi / 180.0, 0.0 * math.pi / 180.0, 0.0 * math.pi / 180.0, 0.0 * math.pi / 180.0 ], [ 20.0 * math.pi / 180.0, 40.0 * math.pi / 180.0, 50.0 * math.pi / 180.0, 50.0 * math.pi / 180.0, 50.0 * math.pi / 180.0 ], [ 0.0 * math.pi / 180.0, 60.0 * math.pi / 180.0, 65.0 * math.pi / 180.0, 65.0 * math.pi / 180.0, 65.0 * math.pi / 180.0 ], [ 80.0 * math.pi / 180.0, 80.0 * math.pi / 180.0, 80.0 * math.pi / 180.0, 15.0 * math.pi / 180.0, 15.0 * math.pi / 180.0 ], [ 80.0 * math.pi / 180.0, -10.0 * math.pi / 180.0, -20.0 * math.pi / 180.0, -20.0 * math.pi / 180.0, -20.0 * math.pi / 180.0 ], [ 80.0 * math.pi / 180.0, 80.0 * math.pi / 180.0, 0.0 * math.pi / 180.0, 0.0 * math.pi / 180.0, 0.0 * math.pi / 180.0 ], [ -80.0 * math.pi / 180.0, -50.0 * math.pi / 180.0, 0.0 * math.pi / 180.0, 0.0 * math.pi / 180.0, 0.0 * math.pi / 180.0 ], [ -20.0 * math.pi / 180.0, -40.0 * math.pi / 180.0, -50.0 * math.pi / 180.0, -50.0 * math.pi / 180.0, -50.0 * math.pi / 180.0 ], [ 0.0 * math.pi / 180.0, -60.0 * math.pi / 180.0, -65.0 * math.pi / 180.0, -65.0 * math.pi / 180.0, -65.0 * math.pi / 180.0 ], [ 80.0 * math.pi / 180.0, 80.0 * math.pi / 180.0, 80.0 * math.pi / 180.0, 80.0 * math.pi / 180.0, 80.0 * math.pi / 180.0 ], [ -25.0 * math.pi / 180.0, -25.0 * math.pi / 180.0, -72.0 * math.pi / 180.0, -72.0 * math.pi / 180.0, -25.0 * math.pi / 180.0 ], [ 40.0 * math.pi / 180.0, 40.0 * math.pi / 180.0, 60.0 * math.pi / 180.0, 60.0 * math.pi / 180.0, 40.0 * math.pi / 180.0 ], [ -18.0 * math.pi / 180.0, -18.0 * math.pi / 180.0, -20.0 * math.pi / 180.0, -20.0 * math.pi / 180.0, -20.0 * math.pi / 180.0 ], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0], [ -25.0 * math.pi / 180.0, -25.0 * math.pi / 180.0, -72.0 * math.pi / 180.0, -72.0 * math.pi / 180.0, -25.0 * math.pi / 180.0 ], [ 40.0 * math.pi / 180.0, 40.0 * math.pi / 180.0, 60.0 * math.pi / 180.0, 60.0 * math.pi / 180.0, 40.0 * math.pi / 180.0 ], [ -18.0 * math.pi / 180.0, -18.0 * math.pi / 180.0, -20.0 * math.pi / 180.0, -20.0 * math.pi / 180.0, -20.0 * math.pi / 180.0 ], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0]] timeLists = [[2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15], [2, 5.0, 8.0, 10, 15]] isAbsolute = True motionProxy.angleInterpolation(names, angleLists, timeLists, isAbsolute) # Activate Whole Body Balancer isEnabled = False motionProxy.wbEnable(isEnabled)