Effector Control

It is useful for reactive behaviour. User can update the target (Head Orientation, Arm Position) and motion is safe and smooth using a SE3 Interpolation.

Note:	Previous cartesian motion are limited because Inverse Kinematic is based only on the joints of the chain. For example, LArm control used only the degree of freedom of the LArm. So, the accessible area is limited to LArm length. Whole Body motion uses all the joints to move an effector. As a consequence, the reachable area is extremely increased.

How it works

The effector target is reach following the next conditions:

Keep the two foot fixed and on the ground;
Keep balance;
Redundancy: reach the target by keeping as possible an init pose.

How to use it

First of all, choose the effector you want to control, with the api wbEnableEffectorControl. It implicitly activate Whole Body Balancer.

# Example showing how to active Head tracking. effectorName = 'Head' isEnabled = True proxy.wbEnableEffectorControl(effectorName, isEnabled)

Update as you want the target with api wbSetEffectorControl:

Head is controlled in rotation (WX, WY, WZ) (radian).
LArm and RArm are controlled in position (X, Y, Z) (meter).

TargetCoordinate must be absolute and expressed in SPACE_NAO.

If the desired position/orientation is unfeasible, target is resize to the nearest feasible motion.


          # Example showing how to set orientation target for Head tracking. 
          effectorName = "Head" 
          targetCoordinate = [0.1, 0.0, 0.0] 
          proxy.wbSetEffectorControl(effectorName, targetCoordinate)

At last, think to disable effector control.

# Example showing how to disactivate Head tracking. effectorName = 'Head' isEnabled = False proxy.wbEnableEffectorControl(effectorName, isEnabled)

Note:	Do an init posture before using this api.

Head Orientation Control

# Example of a whole body head orientation control # Warning: Needs a PoseInit before executing # Whole body balancer must be disactivated at the end of the script # Example available: path/to/aldebaran-sdk/modules/src/examples/ # python/motion_wbEffectorControl_Head.py effectorName = "Head" # Active Head tracking. isEnabled = True proxy.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 targetCoordinateList = [ [+20.0, 00.0, 00.0], # target 0 [-20.0, 00.0, 00.0], # target 1 [ 00.0, +70.0, 00.0], # target 2 [ 00.0, +70.0, +30.0], # target 3 [ 00.0, +70.0, -30.0], # target 4 [ 00.0, -75.0, 00.0], # target 5 [ 00.0, -75.0, +30.0], # target 6 [ 00.0, -75.0, -30.0], # target 7 [ 00.0, 00.0, 00.0], # target 8 ] # wbSetEffectorControl is a non blocking function. # time.sleep allow head go to his target. # The minimum period advised between two successive set commands is 0.2 s. for targetCoordinate in targetCoordinateList: targetCoordinate = [target*math.pi/180.0 for target in targetCoordinate] proxy.wbSetEffectorControl(effectorName, targetCoordinate) time.sleep(3.0) # Disactivate Head tracking. isEnabled = False proxy.wbEnableEffectorControl(effectorName, isEnabled)

Arm Position Control


        
        # Example of a whole body Left Arm position control
        # Warning: Needs a PoseInit before executing
        #          Whole body balancer must be disactivated at the end of the script
        # Example available: path/to/aldebaran-sdk/modules/src/examples/
        #                    python/motion_wbEffectorControl_LArm.py

        effectorName = "LArm"

        space     = motion.SPACE_NAO
        useSensor = False

        effectorInit = proxy.getPosition(effectorName, space, useSensor)

        # Active LArm tracking.
        isEnabled    = True 
        proxy.wbEnableEffectorControl(effectorName, isEnabled)

        # Example showing how to set position target for LArm. 
        # The 3 coordinates are absolute LArm position in NAO_SPACE.
        # Position in meter in x, y and z axis.
        
        # X Axis LArm Position feasible movement = [ +0.00, +0.18] meter
        # Y Axis LArm Position feasible movement = [ -0.04, +0.15] meter
        # Z Axis LArm Position feasible movement = [ -0.10, +0.10] meter    
        
        targetCoordinateList = [
        [ +0.18, +0.00, +0.00], # target 0
        [ +0.18, +0.00, -0.10], # target 1
        [ +0.18, +0.15, -0.10], # target 1
        [ +0.18, +0.15, +0.10], # target 2
        [ +0.18, -0.04, +0.10], # target 3
        [ +0.18, -0.04, -0.10], # target 4
        [ +0.18, +0.00, -0.10], # target 5
        [ +0.18, +0.00, +0.00], # target 6
        [ +0.00, +0.00, +0.00], # target 7
        ]
        
        # wbSetEffectorControl is a non blocking function.
        # time.sleep allow head go to his target.
        # The minimum period advised between two successive set commands is 0.2 s. 
        for targetCoordinate in targetCoordinateList:
            targetCoordinate = [targetCoordinate[i] + effectorInit[i] for i in range(3)]
            proxy.wbSetEffectorControl(effectorName, targetCoordinate) 
            time.sleep(4.0)
        
        # Disactivate Head tracking.
        isEnabled    = False 
        proxy.wbEnableEffectorControl(effectorName, isEnabled)

<< Previous

Next >>