Python ALProxy.wbFootState方法代码示例

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

#.........这里部分代码省略.........
                ["TorsoWy", 0.0]])

    def sidestep(self, value):
        self.motionProxy.setWalkTargetVelocity(0.0,value,0.0,1.0,
                [#LEFT
                ["MaxStepX", 0.07],
                #maxStepYRight,
                ["MaxStepTheta", 0.349],
                ["MaxStepFrequency", 1],
                ["StepHeight", 0.015],
                ["TorsoWx", 0.0],
                ["TorsoWy", 0.0]],
                [#RIGHT
                ["MaxStepX", 0.7],
                #maxStepYLeft,
                ["MaxStepTheta", 0.349],
                ["MaxStepFrequency", 1],
                ["StepHeight", 0.015],
                ["TorsoWx", 0.0], 
                ["TorsoWy", 0.0]])

    def killWalk(self):
        self.motionProxy.stopWalk()

    def leftKick(self, prime, execute, cool_down):

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

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

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

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

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

        # RLeg is optimized
        effectorName = "RLeg"
        axisMask     = 63
        space        = motion.FRAME_ROBOT


        # Motion of the RLeg
        dx      = 0.05                 # translation axis X (meters)
        dz      = 0.05                 # translation axis Z (meters)
        dwy     = 5.0*math.pi/180.0    # rotation axis Y (radian)


        times   = [prime, execute, cool_down]
        isAbsolute = False

        targetList = [

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbFootState [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 wbFootState [as 别名]
from naoqi import ALProxy
import argparse
import almath
import motion
import time

robot_ip = '192.168.1.100'
robot_port = 9559
motionProxy =ALProxy("ALMotion",robot_ip,robot_port)
motionProxy.wakeUp()
enable = True
chainName = "Arms"
isSuccess = motionProxy.setCollisionProtectionEnabled(chainName, enable)
stateName  = "Plane"
supportLeg = "Legs"
motionProxy.wbFootState(stateName, supportLeg)
time.sleep(1)

def lowerReference():
	RHipRoll = -5.72957808107;
	RHipPitch = 2.44845112119;
	RKneePitch = -5.15662040103;
	RAnklePitch = 5.15662040103;
	RAnkleRoll = 7.44845112119;
	LHipRoll = 5.72957808107;
	LHipPitch = 2.44845112119;
	LKneePitch = -5.15662040103;
	LAnklePitch = 5.15662040103;
	LAnkleRoll = -7.44845112119;
	names =["RHipRoll","RHipPitch","RKneePitch","RAnklePitch","RAnkleRoll","LHipRoll","LHipPitch","LKneePitch","LAnklePitch","LAnkleRoll"]
	angles =[RHipRoll*almath.TO_RAD,RHipPitch*almath.TO_RAD,RKneePitch*almath.TO_RAD,RAnklePitch*almath.TO_RAD,RAnkleRoll*almath.TO_RAD,LHipRoll*almath.TO_RAD,LHipPitch*almath.TO_RAD,LKneePitch*almath.TO_RAD,LAnklePitch*almath.TO_RAD,LAnkleRoll*almath.TO_RAD]                        

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbFootState [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 wbFootState [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

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbFootState [as 别名]
def main(robotIP, PORT=9559):
    '''
        Example of a whole body multiple effectors control "LArm", "RArm" and "Torso"
        Warning: Needs a PoseInit before executing
                 Whole body balancer must be inactivated at the end of the script
    '''

    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
    id = postureProxy.post.goToPosture("Stand", 0.5)
    postureProxy.wait(id, 0)

    # end go to Stand Init, begin initialize whole body

    # Enable 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)

    # end initialize whole body, define arms motions

    useSensorValues = False

    # ***************************************************************

    # effectorList = ["LArm"]
    # arm = "LArm"
    # axisDirection = 1    # +1 for LArm, -1 for RArm
    # frame = motion.FRAME_WORLD
    # currentTf = motionProxy.getTransform(arm, frame, useSensorValues)

    for i in range(5):



        print i

        # Arms motion
        effectorList = ["LArm"]
        arm = "LArm"
        axisDirection = 1    # +1 for LArm, -1 for RArm

        frame = motion.FRAME_WORLD
        pathArm = []
        
        currentTf = motionProxy.getTransform(arm, frame, useSensorValues)

        # 1 - arm ready out front
        target1Tf = almath.Transform(currentTf)
        target1Tf.r1_c4 += 0.05 # x
        target1Tf.r2_c4 += 0.00 * axisDirection # y
        target1Tf.r3_c4 += 0.00 # z

        # 2 - arm back
        target2Tf = almath.Transform(currentTf)
        target2Tf.r1_c4 += 0.00
        target2Tf.r2_c4 += 0.15
        target2Tf.r3_c4 += 0.15

        # 3 - arm to ball using ball.y
        target3Tf = almath.Transform(currentTf)
        target3Tf.r1_c4 += 0.05
        target3Tf.r2_c4 += 0.00 * axisDirection
        target3Tf.r3_c4 += 0.10

        pathArm.append(list(target1Tf.toVector()))
        pathArm.append(list(target2Tf.toVector()))
        pathArm.append(list(target3Tf.toVector()))

        pathList = [pathArm]

        axisMaskList = [almath.AXIS_MASK_VEL]

        coef = 1.5
        timesList = [coef * (i + 1) for i in range(len(pathArm))]

        # And move!
        id = motionProxy.post.transformInterpolations(effectorList, frame, pathArm, axisMaskList, timesList)
        motionProxy.wait(id, 0)

        # It is necessary to return the robot to the start position so the next target
        # positions are not added to the last move position.
        # id = postureProxy.post.goToPosture("Stand", 0.75)
        # postureProxy.wait(id, 0)
#.........这里部分代码省略.........