Stabilizing Pepper's head

Good morning,

I have a short program to control pepper's head movements but sometimes the head "springs" back to it's original state. I want pepper's head to stay at it's last position until I send the next command.

The pepper's head stays in it's given position until she sees a person. After that it seems as though autonomous life is kicked in but printed statements say otherwise.

Also, is it possible to control pepper's head movement via ROS? If so, how? Thanks

#!/usr/bin/env python

# -*- encoding: UTF-8 -*-

import time
import almath
import argparse
from naoqi import ALProxy
from pprint import pprint

tts = None
motion = None
posture = None
autonomouslife = None
basicawareness = False
fractionMaxSpeed = 0.1

def init(robotIP, PORT):
    global tts, motion, posture, autonomouslife, basicawareness
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    autonomouslife = ALProxy("ALAutonomousLife", robotIP, PORT)
    basicawareness = ALProxy("ALBasicAwareness", robotIP, PORT)
    motion = ALProxy("ALMotion", robotIP, PORT)
    posture = ALProxy("ALRobotPosture", robotIP, PORT)

def say(something_to_say):
    tts.say(str(something_to_say))

def standup():
    say('Standing up')
    posture.goToPosture("Stand", 5)
    time.sleep(4.0)
    say('I am ready')

def set_head_stiffness(value=0):
    say('Setting heading stiffness to ' + str(value))
    motion.setStiffnesses("Head", value)
    time.sleep(1.0)

def head_yaw(angle):

    names = "HeadYaw"
    angles = angle * almath.TO_RAD
    say('setting head yaw to ' + str(angle) + 'degrees')
    # motion.setAngles(names, angles, fractionMaxSpeed)
    motion.angleInterpolation(names, angles, 3.0, True)
    time.sleep(2.0)

    say('done head yaw')

def head_pitch(angle):

    names = "HeadPitch"
    angles = angle * almath.TO_RAD
    say('setting head pitch to ' + str(angle) + 'degrees')
    # motion.setAngles(names, angles, fractionMaxSpeed)
    motion.angleInterpolation(names, angles, 3.0, True)
    time.sleep(2.0)

    say('done head pitch')

def check_auto_life():
    st = autonomouslife.getAutonomousAbilitiesStatus()
    pprint(st)
    print '\n\n'
    tts.say('auto life is ' + autonomouslife.getState())

def main(angle, robotIP, PORT=9559):
    init(robotIP, PORT)
    al_state = autonomouslife.getState()
    if al_state != 'disabled':
        say('auto life is ' + al_state)
        autonomouslife.setState('disabled')

    check_auto_life()

    tts.say('disabling basic life awareness')
    basicawareness.setEnabled(False)
    basicawareness.pauseAwareness()
    basicawareness.setStimulusDetectionEnabled('People', False)

    standup()
    set_head_stiffness(0.5)
    head_pitch(0)
    head_yaw(0)

    check_auto_life()

    head_pitch(angle)
    check_auto_life()
    head_pitch(0)

    head_yaw(angle)
    check_auto_life()
    head_yaw(0)
    head_pitch(0)
    w = 3
    say('Waiting for ' + str(w) + ' seconds')
    check_auto_life()
    time.sleep(w)
    say('Bye bye ')
    check_auto_life()

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--ip", type=str, default="192.168.11.182",
                        help="Robot ip address")
    parser.add_argument("--port", type=int, default=9559,
                        help="Robot port number")
    parser.add_argument("--angle", type=float, default=0,
                        help="Head angle in degrees")

    args = parser.parse_args()
    main(args.angle, args.ip, args.port)

ros-naoqi / naoqi_bridge

Stabilizing Pepper's head #93