Python ALProxy.transformInterpolations方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import transformInterpolations [as 别名]
def main(robotIP, PORT=9559):
    ''' Example showing a path of two positions
    Warning: Needs a PoseInit before executing
    '''

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    effector   = "LArm"
    frame      = motion.FRAME_TORSO
    axisMask   = almath.AXIS_MASK_VEL # just control position
    useSensorValues = False

    path = []
    currentTf = motionProxy.getTransform(effector, frame, useSensorValues)
    targetTf  = almath.Transform(currentTf)
    targetTf.r1_c4 += 0.03 # x
    targetTf.r2_c4 += 0.03 # y

    path.append(list(targetTf.toVector()))
    path.append(currentTf)

    # Go to the target and back again
    times      = [2.0, 4.0] # seconds

    motionProxy.transformInterpolations(effector, frame, path, axisMask, times)

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import transformInterpolations [as 别名]
def main(robotIP, PORT=9559):
    ''' Example showing a hand ellipsoid
    Warning: Needs a PoseInit before executing
    '''

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    #tts = ALProxy("ALTextToSpeech", "192.168.2.17", 9559)
    tts = ALProxy("ALTextToSpeech", "168.156.34.195", 9559)
    tts.say("Hello, I'm going to move my left arm.")
    tts.say("Then I will go back to the rest position!")
    
    effector   = "LArm"
    frame      = motion.FRAME_TORSO
    axisMask   = almath.AXIS_MASK_VEL    # just control position
    useSensorValues = False

    path = []
    currentTf = motionProxy.getTransform(effector, frame, useSensorValues)
    # point 1
    targetTf  = almath.Transform(currentTf)
    targetTf.r2_c4 -= 0.05 # y
    path.append(list(targetTf.toVector()))

    # point 2
    targetTf  = almath.Transform(currentTf)
    targetTf.r3_c4 += 0.04 # z
    path.append(list(targetTf.toVector()))

    # point 3
    targetTf  = almath.Transform(currentTf)
    targetTf.r2_c4 += 0.04 # y
    path.append(list(targetTf.toVector()))

    # point 4
    targetTf  = almath.Transform(currentTf)
    targetTf.r3_c4 -= 0.02 # z
    path.append(list(targetTf.toVector()))

    # point 5
    targetTf  = almath.Transform(currentTf)
    targetTf.r2_c4 -= 0.05 # y
    path.append(list(targetTf.toVector()))

    # point 6
    path.append(currentTf)

    times = [0.5, 1.0, 2.0, 3.0, 4.0, 4.5] # seconds

    motionProxy.transformInterpolations(effector, frame, path, axisMask, times)

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import transformInterpolations [as 别名]
def main(robotIP, PORT=9559):
    ''' Example of a cartesian foot trajectory
    Warning: Needs a PoseInit before executing
    '''

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    frame      = motion.FRAME_WORLD
    axisMask   = almath.AXIS_MASK_ALL   # full control
    useSensorValues = False

    # Lower the Torso and move to the side
    effector = "Torso"
    initTf   = almath.Transform(
        motionProxy.getTransform(effector, frame, useSensorValues))
    deltaTf  = almath.Transform(0.0, -0.06, -0.03) # x, y, z
    targetTf = initTf*deltaTf
    path     = list(targetTf.toVector())
    times    = 2.0 # seconds
    motionProxy.transformInterpolations(effector, frame, path, axisMask, times)

    # LLeg motion
    effector = "LLeg"
    initTf = almath.Transform()

    try:
        initTf = almath.Transform(motionProxy.getTransform(effector, frame, useSensorValues))
    except Exception, errorMsg:
        print str(errorMsg)
        print "This example is not allowed on this robot."
        exit()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import transformInterpolations [as 别名]
def main(robotIP, PORT=9559):
    '''
         Example showing a Hula Hoop Motion
         with the NAO cartesian control of torso
    '''

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # end initialize proxy, begin go to Stand Init

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    # end go to Stand Init, begin define control point
    effector        = "Torso"
    frame           =  motion.FRAME_ROBOT
    axisMask        = almath.AXIS_MASK_ALL
    isAbsolute      = True
    useSensorValues = False

    currentTf = almath.Transform(motionProxy.getTransform(effector, frame, useSensorValues))

    # end define control point, begin define target

    # Define the changes relative to the current position
    dx         = 0.03                    # translation axis X (meter)
    dy         = 0.03                    # translation axis Y (meter)
    dwx        = 8.0*almath.TO_RAD       # rotation axis X (rad)
    dwy        = 8.0*almath.TO_RAD       # rotation axis Y (rad)

    # point 01 : forward  / bend backward
    target1Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
    target1Tf *= almath.Transform(dx, 0.0, 0.0)
    target1Tf *= almath.Transform().fromRotY(-dwy)

    # point 02 : right    / bend left
    target2Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
    target2Tf *= almath.Transform(0.0, -dy, 0.0)
    target2Tf *= almath.Transform().fromRotX(-dwx)

    # point 03 : backward / bend forward
    target3Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
    target3Tf *= almath.Transform(-dx, 0.0, 0.0)
    target3Tf *= almath.Transform().fromRotY(dwy)

    # point 04 : left     / bend right
    target4Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
    target4Tf *= almath.Transform(0.0, dy, 0.0)
    target4Tf *= almath.Transform().fromRotX(dwx)

    path = []
    path.append(list(target1Tf.toVector()))
    path.append(list(target2Tf.toVector()))
    path.append(list(target3Tf.toVector()))
    path.append(list(target4Tf.toVector()))

    path.append(list(target1Tf.toVector()))
    path.append(list(target2Tf.toVector()))
    path.append(list(target3Tf.toVector()))
    path.append(list(target4Tf.toVector()))

    path.append(list(target1Tf.toVector()))
    path.append(list(currentTf.toVector()))

    timeOneMove  = 0.5 #seconds
    times = []
    for i in range(len(path)):
        times.append((i+1)*timeOneMove)

    # end define target, begin call motion api

    # call the cartesian control API

    motionProxy.transformInterpolations(effector, frame, path, axisMask, times)

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import transformInterpolations [as 别名]
 def run(self, robotIP):
     ''' Example of a whole body kick
      Warning: Needs a PoseInit before executing
              Whole body balancer must be inactivated at the end of the script
     '''
 
     motionProxy  = ALProxy("ALMotion", robotIP, 9559)
     postureProxy = ALProxy("ALRobotPosture", robotIP, 9559)
 
     # Wake up robot
     motionProxy.wakeUp()
 
     # Send robot to Stand Init
     postureProxy.goToPosture("StandInit", 0.5)
 
     # Activate Whole Body Balancer
     isEnabled  = True
     motionProxy.wbEnable(isEnabled)
 
     # Legs are constrained fixed
     stateName  = "Fixed"
     supportLeg = "Legs"
     motionProxy.wbFootState(stateName, supportLeg)
 
     # Constraint Balance Motion
     isEnable   = True
     supportLeg = "Legs"
     motionProxy.wbEnableBalanceConstraint(isEnable, supportLeg)
 
     # Com go to LLeg
     supportLeg = "LLeg"
     duration   = 2.0
     motionProxy.wbGoToBalance(supportLeg, duration)
 
     # RLeg is free
     stateName  = "Free"
     supportLeg = "RLeg"
     motionProxy.wbFootState(stateName, supportLeg)
 
     # RLeg is optimized
     effector = "RLeg"
     axisMask = 63
     frame    = motion.FRAME_WORLD
 
     # Motion of the RLeg
     times   = [2.0, 2.7, 4.5]
 
     path = Kick.computePath(self, motionProxy, effector, frame)
 
     motionProxy.transformInterpolations(effector, frame, path, axisMask, times)
 
     # Example showing how to Enable Effector Control as an Optimization
     isActive     = False
     motionProxy.wbEnableEffectorOptimization(effector, isActive)
 
     # Com go to LLeg
     supportLeg = "RLeg"
     duration   = 2.0
     motionProxy.wbGoToBalance(supportLeg, duration)
 
     # RLeg is free
     stateName  = "Free"
     supportLeg = "LLeg"
     motionProxy.wbFootState(stateName, supportLeg)
 
     effector = "LLeg"
     path = Kick.computePath(self, motionProxy, effector, frame)
     motionProxy.transformInterpolations(effector, frame, path, axisMask, times)
 
     time.sleep(1.0)
 
     # Deactivate Head tracking
     isEnabled = False
     motionProxy.wbEnable(isEnabled)
 
     # send robot to Pose Init
     postureProxy.goToPosture("StandInit", 0.3)
 
     # Go to rest position
     motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import transformInterpolations [as 别名]
def main(robotIP, PORT=9559):
    ''' Simultaneously control three effectors:
    the Torso, the Left Arm and the Right Arm
    Warning: Needs a PoseInit before executing
    '''

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    frame      = motion.FRAME_WORLD
    coef       = 0.5                   # motion speed
    times      = [coef, 2.0*coef, 3.0*coef, 4.0*coef]
    useSensorValues = False

    # Relative movement between current and desired positions
    dy         = +0.03                 # translation axis Y (meters)
    dz         = -0.03                 # translation axis Z (meters)
    dwx        = +8.0*almath.TO_RAD   # rotation axis X (radians)

    # Motion of Torso with post process
    effector   = "Torso"

    path = []
    initTf = almath.Transform(motionProxy.getTransform(effector, frame, useSensorValues))
    # point 1
    deltaTf  = almath.Transform(0.0, -dy, dz)*almath.Transform().fromRotX(-dwx)
    targetTf = initTf*deltaTf
    path.append(list(targetTf.toVector()))

    # point 2
    path.append(list(initTf.toVector()))

    # point 3
    deltaTf  = almath.Transform(0.0, dy, dz)*almath.Transform().fromRotX(dwx)
    targetTf = initTf*deltaTf
    path.append(list(targetTf.toVector()))

    # point 4
    path.append(list(initTf.toVector()))

    axisMask   = almath.AXIS_MASK_ALL  # control all the effector axes
    motionProxy.post.transformInterpolations(effector, frame, path,
                                           axisMask, times)

    # Motion of Arms with block process
    frame     = motion.FRAME_TORSO
    axisMask  = almath.AXIS_MASK_VEL  # 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 = []
    currentTf = motionProxy.getTransform(effector, frame, useSensorValues)
    targetTf  = almath.Transform(currentTf)
    targetTf.r2_c4 -= 0.04 # y
    path.append(list(targetTf.toVector()))
    path.append(currentTf)

    motionProxy.transformInterpolations(effector, frame, path, axisMask, times)

    # Motion of Left Arm during the last half of the Torso motion
    effector   = "LArm"

    path = []
    currentTf = motionProxy.getTransform(effector, frame, useSensorValues)
    targetTf  = almath.Transform(currentTf)
    targetTf.r2_c4 += 0.04 # y
    path.append(list(targetTf.toVector()))
    path.append(currentTf)

    motionProxy.transformInterpolations(effector, frame, path, axisMask, times)

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import transformInterpolations [as 别名]
def main(robotIP, PORT=9559):
    ''' Move the torso and keep arms fixed in nao space
    Warning: Needs a PoseInit before executing
    '''

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    frame      = motion.FRAME_ROBOT
    useSensorValues = False

    effectorList = ["LArm", "RArm"]

    # Motion of Arms with block process
    axisMaskList = [almath.AXIS_MASK_VEL, almath.AXIS_MASK_VEL]

    timesList    = [[1.0], [1.0]] # seconds

    # LArm path
    pathLArm = []
    targetTf  = almath.Transform(motionProxy.getTransform("LArm", frame, useSensorValues))
    targetTf.r2_c4 -= 0.04 # y
    pathLArm.append(list(targetTf.toVector()))

    # RArm path
    pathRArm = []
    targetTf  = almath.Transform(motionProxy.getTransform("RArm", frame, useSensorValues))
    targetTf.r2_c4 += 0.04 # y
    pathRArm.append(list(targetTf.toVector()))

    pathList = []
    pathList.append(pathLArm)
    pathList.append(pathRArm)

    motionProxy.transformInterpolations(effectorList, frame, pathList,
                                       axisMaskList, timesList)

    effectorList = ["LArm", "RArm", "Torso"]

    # Motion of Arms and Torso with block process
    axisMaskList = [almath.AXIS_MASK_VEL,
                    almath.AXIS_MASK_VEL,
                    almath.AXIS_MASK_ALL]

    timesList    = [[4.0],                  # LArm  in seconds
                    [4.0],                  # RArm  in seconds
                    [1.0, 2.0, 3.0, 4.0]]   # Torso in seconds

    # LArm path
    pathLArm = []
    pathLArm.append(motionProxy.getTransform("LArm", frame, useSensorValues))

    # RArm path
    pathRArm = []
    pathRArm.append(motionProxy.getTransform("RArm", frame, useSensorValues))

    # Torso path
    pathTorso = []
    currentTf = motionProxy.getTransform("Torso", frame, useSensorValues)

    # 1
    targetTf  = almath.Transform(currentTf)
    targetTf.r2_c4 += 0.04 # y
    pathTorso.append(list(targetTf.toVector()))

    # 2
    targetTf  = almath.Transform(currentTf)
    targetTf.r1_c4 -= 0.03 # x
    pathTorso.append(list(targetTf.toVector()))

    # 3
    targetTf  = almath.Transform(currentTf)
    targetTf.r2_c4 -= 0.04 # y
    pathTorso.append(list(targetTf.toVector()))

    # 4
    pathTorso.append(currentTf)

    pathList = []
    pathList.append(pathLArm)
    pathList.append(pathRArm)
    pathList.append(pathTorso)

    motionProxy.transformInterpolations(effectorList, frame, pathList,
                                       axisMaskList, timesList)

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import transformInterpolations [as 别名]

#.........这里部分代码省略.........
            # Compute the position of the ball relative to the robot
            self.ball[0] = camera_pos[2] * tan(camera_pos[4] + ball_info[1] + ball_info[2]) + camera_pos[0]
            self.ball[1] = camera_pos[2] * tan(camera_pos[3]) + camera_pos[1]

            # Compute the distance between the robot and the ball
            dist = camera_pos[2] * tan(camera_pos[4] + ball_info[1])
            # Compare to the euclidean distance
            dist_eucl = sqrt(self.ball[0]**2 + self.ball[1]**2)
            print("Distance difference: "+str(dist - dist_eucl))

            # If the robot is near the ball enough (set to 2 centimeters here)
            if dist < 0.02:
                # Stop the tracker
                self.tracker.stopTracker()
                # TODO: Request the robot to look for the landmark

        # Warn that we found a red ball
        print("RED BALL DETECTED " + str(self.ball))

    def on_landmark_detected(self):
        """Compute the position of the landmark relative to the robot"""

        # If the tracker is stopped or not following the landmark
        if not self.tracker.isActive() or self.tracker.getActiveTarget() != "LandMark":
            # Stop the tracker
            self.tracker.stopTracker()

            # Ask the robot to track the target by moving only its body
            self.tracker.setMode("WholeBody")
            # Start to track the landmark
            self.tracker.track("LandMark")
        else:
            # Get the data from the memory
            data = memory.getData("LandmarkDetected")
            # Check that we have the right mark
            if data[1][0][1] == mark_id:
                # Extract information regarding the mark
                mark_info = data[1][0][0]
                camera_pos = data[3]
                # Compute the position of the landmark relative to the robot
                self.goal[0] = camera_pos[2] * tan(camera_pos[4] + mark_info[2] + mark_info[3]) + camera_pos[0]
                self.goal[1] = camera_pos[2] * tan(camera_pos[3]) + camera_pos[1]
                # Turn the robot toward the goal
                rotation = atan((self.goal[0] - self.ball[0]) / (self.goal[1] - self.ball[1]))

                # If the robot is facing the landmark with a 2 degree precision
                if -2 * almath.TO_RAD < rotation < 2 * almath.TO_RAD:
                    # Stop the tracker
                    self.tracker.stopTracker()
                    # TODO: Request the robot to kick

                # Warn that we found a landmark
                print("LANDMARK DETECTED " + str(self.goal))

    def on_fall(self):
        """Set the stiffness back and request the robot to get up"""

        # Set the body stiffness
        self.motion.setStiffnesses("Body", 1)
        # Ask the robot to stand up
        self.posture.goToPosture("StandInit", 0.5)

    def on_bumper_pressed(self):
        """Have the robot turn around and walk away"""

        # Stop all movements
        self.motion.stopMove()
        # Turn around
        self.motion.moveTo(0, 0, pi / 2)

    def kick(self):
        """Have the robot execute a kick"""

        # Request robot to go to stand init posture
        self.posture.goToPosture("StandInit", 0.5)

        # Activate whole body balancer
        self.motion.wbEnable(True)

        # Constraint both legs as fixed
        self.motion.wbFootState("Fixed", "Legs")

        # Request legs joints to move to keep robot's balance
        self.motion.wbEnableBalanceConstraint(True, "Legs")

        # Balance the robot on the left leg
        self.motion.wbGoToBalance("LLeg", 2.5)

        # Remove fixed constraint from the right leg
        self.motion.wbFootState("Free", "RLeg")

        # Compute the path for moving the right leg back and forth
        path = compute_path(self.motion, "RLeg", motion.FRAME_WORLD)

        # Request the right leg to move following the computed path
        times = [1.2, 1.6, 2.3]
        self.motion.transformInterpolations("RLeg", motion.FRAME_WORLD, path, 63, times)

        # Request the robot to stand in initial position
        self.posture.goToPosture("StandInit", 0.2)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import transformInterpolations [as 别名]
def kick():
    motionProxy  = ALProxy("ALMotion", nao_host, nao_port)
    postureProxy = ALProxy("ALRobotPosture", nao_host, nao_port)
    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Activate Whole Body Balancer
    isEnabled  = True
    motionProxy.wbEnable(isEnabled)

    # Legs are constrained fixed
    stateName  = "Fixed"
    supportLeg = "Legs"
    motionProxy.wbFootState(stateName, supportLeg)

    # Constraint Balance Motion
    isEnable   = True
    supportLeg = "Legs"
    motionProxy.wbEnableBalanceConstraint(isEnable, supportLeg)

    # Com go to LLeg
    supportLeg = "LLeg"
    duration   = 2.0
    motionProxy.wbGoToBalance(supportLeg, duration)

    # RLeg is free
    stateName  = "Free"
    supportLeg = "RLeg"
    motionProxy.wbFootState(stateName, supportLeg)

    # RLeg is optimized
    effector = "RLeg"
    axisMask = 63
    frame    = motion.FRAME_WORLD

    # Motion of the RLeg
    times   = [2.0, 2.7, 4.5]

    path = computePath(motionProxy, effector, frame)

    motionProxy.transformInterpolations(effector, frame, path, axisMask, times)

    # Example showing how to Enable Effector Control as an Optimization
    isActive     = False
    motionProxy.wbEnableEffectorOptimization(effector, isActive)

    # Com go to LLeg
    supportLeg = "RLeg"
    duration   = 2.0
    motionProxy.wbGoToBalance(supportLeg, duration)

    # RLeg is free
    stateName  = "Free"
    supportLeg = "LLeg"
    motionProxy.wbFootState(stateName, supportLeg)

    effector = "LLeg"
    path = computePath(motionProxy, effector, frame)
    motionProxy.transformInterpolations(effector, frame, path, axisMask, times)

    time.sleep(1.0)

    # Deactivate Head tracking
    isEnabled = False
    motionProxy.wbEnable(isEnabled)

    # send robot to Pose Init
    postureProxy.goToPosture("StandInit", 0.3)

    # Go to rest position
    motionProxy.rest()
    return "ZzzZzzZzzZ", 200