315 lines
10 KiB
Python
315 lines
10 KiB
Python
"""Class with functions for easier movement of NAO.
|
|
|
|
And a command line script for some movements.
|
|
"""
|
|
|
|
# from time import sleep
|
|
import argparse
|
|
from math import radians, pi
|
|
|
|
from naoqi import ALProxy
|
|
|
|
from .utils import read_config
|
|
|
|
class NaoMover(object):
|
|
|
|
KICK_SEQUENCE = [ # DON'T USE THIS ONE
|
|
|
|
# lean to the side using the ankle joints
|
|
[[(0, 1, 'ShoulderRoll', -70),
|
|
(0 , 1, 'AnkleRoll', -9),
|
|
(1 , 1 , 'AnkleRoll', -9)], 0.3],
|
|
|
|
# perform the fast kick
|
|
[[(1, 0, 'HipPitch', -45),
|
|
(1, 0, 'AnklePitch', 10),
|
|
(1, 0, 'KneePitch', 10)], 0.15],
|
|
|
|
# bring knee back for better kick recovery
|
|
[[(1, 0, 'KneePitch', 40)], 0.2]
|
|
]
|
|
|
|
# fancy kick
|
|
KICK_SEQUENCE_FANCY = [ # USE THIS ONE
|
|
# base_or_kicking, unsymmetric, joint, angle
|
|
|
|
# lift the arm
|
|
# [[(0, 1, 'ShoulderRoll', -70)], 0.5],
|
|
|
|
# lean to the side using the ankle joints
|
|
[[(0, 1, 'AnkleRoll', -12),
|
|
(1, 1, 'AnkleRoll', -12)],
|
|
1],
|
|
|
|
# lift the foot using the knee joint and the ankle joint
|
|
[[(1, 0, 'KneePitch', 90),
|
|
(1, 0, 'AnklePitch', -40)],
|
|
0.7],
|
|
|
|
# kick-it!
|
|
[[(1, 0, 'HipPitch', -45),
|
|
(1, 0, 'KneePitch', 10),
|
|
(1, 0, 'AnklePitch', 0)],
|
|
0.3],
|
|
|
|
# prepare to return into standing position
|
|
[[(1, 0, 'KneePitch', 40),
|
|
(1, 0, 'AnklePitch', 10)],
|
|
0.5],
|
|
]
|
|
|
|
def __init__(self, nao_ip, nao_port=9559):
|
|
nao_ip = bytes(nao_ip)
|
|
self.mp = ALProxy('ALMotion', nao_ip, nao_port)
|
|
self.pp = ALProxy('ALRobotPosture', nao_ip, nao_port)
|
|
ap = ALProxy("ALAutonomousLife", nao_ip, nao_port)
|
|
if ap.getState() != "disabled":
|
|
ap.setState('disabled')
|
|
self.set_head_stiffness()
|
|
self.set_hand_stiffness()
|
|
self.set_arm_stiffness()
|
|
self.set_hip_stiffness()
|
|
self.set_knee_stiffness()
|
|
self.set_ankle_stiffness()
|
|
self.ready_to_move = False
|
|
|
|
def kick(self, foot='L', fancy=False):
|
|
"""Kick the ball with the foot.
|
|
|
|
For now optimized for Left foot. Also please always
|
|
set fancy to True when calling this.
|
|
|
|
"""
|
|
self.set_arm_stiffness(0.8)
|
|
self.set_hip_stiffness(0.8)
|
|
self.set_knee_stiffness(0.8)
|
|
self.set_ankle_stiffness(1)
|
|
if foot == 'L':
|
|
sides = ['R', 'L']
|
|
elif foot == 'R':
|
|
sides = ['L', 'R']
|
|
kick_sequence = (self.KICK_SEQUENCE_FANCY if fancy
|
|
else self.KICK_SEQUENCE)
|
|
for motion, wait in kick_sequence:
|
|
joints = []
|
|
angles = []
|
|
for part in motion:
|
|
print(part)
|
|
side, unsymmetric, joint, angle = part
|
|
if foot == 'R' and unsymmetric:
|
|
angle *= -1
|
|
joints.append(sides[side] + joint)
|
|
angles.append(radians(angle))
|
|
self.mp.angleInterpolation(joints, angles, wait, True)
|
|
|
|
self.stand_up(0.5)
|
|
self.set_arm_stiffness()
|
|
self.set_hip_stiffness()
|
|
self.set_knee_stiffness()
|
|
self.set_ankle_stiffness()
|
|
|
|
def stand_up(self, speed=0.8):
|
|
self.set_arm_stiffness(0.9)
|
|
self.set_hand_stiffness(0.9)
|
|
self.pp.goToPosture('StandInit', speed)
|
|
self.set_hand_stiffness()
|
|
self.set_arm_stiffness()
|
|
|
|
def rest(self):
|
|
"""Send robot to resting position."""
|
|
self.mp.rest()
|
|
self.ready_to_move = False
|
|
|
|
def set_head_stiffness(self, stiffness=0.5):
|
|
self.mp.setStiffnesses("Head", stiffness)
|
|
|
|
def set_hand_stiffness(self, stiffness=0.0):
|
|
self.mp.setStiffnesses("RHand", stiffness)
|
|
self.mp.setStiffnesses("LHand", stiffness)
|
|
self.mp.setStiffnesses("LWristYaw", stiffness)
|
|
self.mp.setStiffnesses("RWristYaw", stiffness)
|
|
|
|
def set_arm_stiffness(self, stiffness=0.3):
|
|
self.mp.setStiffnesses("LShoulderPitch", stiffness)
|
|
self.mp.setStiffnesses("LShoulderRoll", stiffness)
|
|
self.mp.setStiffnesses("LElbowRoll", stiffness)
|
|
self.mp.setStiffnesses("LElbowYaw", stiffness)
|
|
self.mp.setStiffnesses("RShoulderPitch", stiffness)
|
|
self.mp.setStiffnesses("RShoulderRoll", stiffness)
|
|
self.mp.setStiffnesses("RElbowRoll", stiffness)
|
|
self.mp.setStiffnesses("RElbowYaw", stiffness)
|
|
|
|
def set_hip_stiffness(self, stiffness=0.6):
|
|
self.mp.setStiffnesses("LHipYawPitch", stiffness)
|
|
self.mp.setStiffnesses("LHipPitch", stiffness)
|
|
self.mp.setStiffnesses("RHipYawPitch", stiffness)
|
|
self.mp.setStiffnesses("RHipPitch", stiffness)
|
|
self.mp.setStiffnesses("LHipRoll", stiffness)
|
|
self.mp.setStiffnesses("RHipRoll", stiffness)
|
|
|
|
def set_ankle_stiffness(self, stiffness=0.6):
|
|
self.mp.setStiffnesses("LAnklePitch", stiffness)
|
|
self.mp.setStiffnesses("LAnkleRoll", stiffness)
|
|
self.mp.setStiffnesses("RAnklePitch", stiffness)
|
|
self.mp.setStiffnesses("RAnkleRoll", stiffness)
|
|
|
|
def set_knee_stiffness(self, stiffness=0.6):
|
|
self.mp.setStiffnesses("LKneePitch", stiffness)
|
|
self.mp.setStiffnesses("RKneePitch", stiffness)
|
|
|
|
def get_head_angles(self):
|
|
return self.mp.getAngles(('HeadYaw', 'HeadPitch'), False)
|
|
|
|
def change_head_angles(self, d_yaw, d_pitch, speed=0.5):
|
|
"""Change the head angles by a relative amount.
|
|
|
|
This function DOES return before movement is finished.
|
|
"""
|
|
self.mp.changeAngles(('HeadYaw', 'HeadPitch'),
|
|
(d_yaw, d_pitch), speed)
|
|
|
|
def change_head_angles_blocking(self, d_yaw, d_pitch, speed=0.5):
|
|
"""Same as `change_head_angles` but block until finished.
|
|
|
|
Doesn't work quite as expected though. You may need to work on it.
|
|
|
|
"""
|
|
self.mp.angleInterpolatioWithSpeed(('HeadYaw', 'HeadPitch'),
|
|
(d_yaw, d_pitch), speed, False)
|
|
|
|
def set_head_angles(self, yaw, pitch, speed=0.5):
|
|
"""Rotate head to a specified angle in the robot frame.
|
|
|
|
This function DOES return before movement is finished.
|
|
"""
|
|
self.mp.setAngles(('HeadYaw', 'HeadPitch'),
|
|
(yaw, pitch), speed)
|
|
|
|
def set_head_angles_blocking(self, yaw, pitch, speed=0.5):
|
|
"""Same as `set_head_angles` but block until finished.
|
|
|
|
Doesn't work quite as expected though. You may need to work on it.
|
|
|
|
"""
|
|
self.mp.angleInterpolatioWithSpeed(('HeadYaw', 'HeadPitch'),
|
|
(yaw, pitch), speed, True)
|
|
|
|
def move_to(self, front, side, rotation, wait=False):
|
|
"""Move the robot around.
|
|
|
|
If `wait` is set, will block until move is finished.
|
|
"""
|
|
if not self.ready_to_move:
|
|
self.mp.moveInit()
|
|
self.ready_to_move = True
|
|
if rotation != 0:
|
|
n_cycles = int(abs(rotation) // (pi/4))
|
|
sign = 1 if rotation > 0 else -1
|
|
rest = abs(rotation) % (pi/4)
|
|
print('Rotation', rotation, 'Cycles', n_cycles, 'Rest', rest)
|
|
for _ in range(n_cycles):
|
|
self.mp.post.moveTo(0, 0, pi/4 * sign)
|
|
self.wait()
|
|
self.mp.post.moveTo(0, 0, rest * sign)
|
|
else:
|
|
self.mp.post.moveTo(front, side, rotation)
|
|
|
|
def move_to_fast(self, front, side, rotation, wait=False):
|
|
"""Move faster. DON'T USE."""
|
|
if not self.ready_to_move:
|
|
self.mp.moveInit()
|
|
self.ready_to_move = True
|
|
self.mp.post.moveTo(front, side, rotation, [
|
|
['MaxStepX', 0.07999999821186066],
|
|
['MaxStepY', 0.1599999964237213],
|
|
['MaxStepTheta', 0.5235987901687622],
|
|
['MaxStepFrequency', 1.0],
|
|
['StepHeight', 0.02]
|
|
])
|
|
|
|
def move_toward(self, front, side, rotation):
|
|
"""Move in a specified direction until interrupted.
|
|
|
|
DON'T USE, unstable, I think.
|
|
"""
|
|
if not self.ready_to_move:
|
|
self.mp.moveInit()
|
|
self.ready_to_move = True
|
|
self.mp.post.moveToward(front, side, rotation)
|
|
|
|
def dance(self):
|
|
dance_sequence = [[["LLeg"], [[0.06, 0.1, 0.0]]],
|
|
[["LLeg"], [[0.00, 0.16, 0.0]]],
|
|
[["RLeg"], [[0.00, -0.1, 0.0]]],
|
|
[["LLeg"], [[0.00, 0.16, 0.0]]],
|
|
[["RLeg"], [[-0.04, -0.1, 0.0]]],
|
|
[["RLeg"], [[0.00, -0.16, 0.0]]],
|
|
[["LLeg"], [[0.00, 0.1, 0.0]]],
|
|
[["RLeg"], [[0.00, -0.16, 0.0]]]]
|
|
|
|
step_frequency = 0.8
|
|
clear_existing = False
|
|
cycles = 2 # defined the number of cycle to make
|
|
|
|
for _ in range(cycles):
|
|
for leg, positions in dance_sequence:
|
|
self.mp.setFootStepsWithSpeed(
|
|
leg, positions, (step_frequency,),
|
|
clear_existing
|
|
)
|
|
self.wait()
|
|
self.stand_up(0.7)
|
|
|
|
def wait(self):
|
|
"""Shortcut for mp.waitUntilMoveIsFinished."""
|
|
self.mp.waitUntilMoveIsFinished()
|
|
|
|
def stop_moving(self):
|
|
print('STOOOP')
|
|
self.mp.stopMove()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
parser = argparse.ArgumentParser()
|
|
actions = parser.add_mutually_exclusive_group()
|
|
actions.add_argument("-s", "--stand", action="store_true",
|
|
help="let the robot stand up")
|
|
actions.add_argument("-k", "--kick", action="store_true",
|
|
help="let the robot do a fancy kick")
|
|
actions.add_argument("-r", "--rest", action="store_true",
|
|
help="let the robot rest")
|
|
actions.add_argument("-m", "--move", action="store_true",
|
|
help="move around")
|
|
actions.add_argument("-d", "--dance", action="store_true",
|
|
help="blow up the dance floor")
|
|
|
|
args = parser.parse_args()
|
|
|
|
cfg = read_config()
|
|
mover = NaoMover(cfg['ip'], cfg['port'])
|
|
|
|
if args.stand:
|
|
mover.stand_up()
|
|
|
|
elif args.rest:
|
|
mover.rest()
|
|
|
|
# perform a (fancy) kick
|
|
elif args.kick:
|
|
mover.stand_up()
|
|
mover.kick(fancy=True)
|
|
|
|
elif args.move:
|
|
mover.stand_up()
|
|
while True:
|
|
amount_x = float(raw_input('How much x: '))
|
|
amount_y = float(raw_input('How much y: '))
|
|
amount_z = float(raw_input('How much z: '))
|
|
print(amount_x, amount_y, amount_z)
|
|
mover.move_to(amount_x, amount_y, amount_z)
|
|
mover.wait()
|
|
|
|
elif args.dance:
|
|
mover.stand_up()
|
|
mover.dance()
|