Stabilizing Pepper's head

edit

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, it is 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
fractionMaxSpeed = 0.1


def init(robotIP, PORT):
    global tts, motion, posture, autonomouslife
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    autonomouslife = ALProxy("ALAutonomousLife", 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 = 1.0
    say('Setting heading stiffness to ' + str(value))
    motion.setStiffnesses("Head", value)
    time.sleep(4.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(4.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(4.0)
    say('done head pitch')


def check_auto_life():
    st = autonomouslife.getAutonomousAbilitiesStatus()
    pprint(st)
    print '\n\n'

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('auto life is ' + autonomouslife.getState())
    standup()
    check_auto_life()
    tts.say('auto life is ' + autonomouslife.getState())

    set_head_stiffness()
    check_auto_life()
    tts.say('auto life is ' + autonomouslife.getState())

    head_pitch(angle)
    check_auto_life()
    tts.say('auto life is ' + autonomouslife.getState())

    head_yaw(angle)
    check_auto_life()
    tts.say('auto life is ' + autonomouslife.getState())

    w = 20
    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)