From 949f657a75f51f2f01c94009b7b202487379a0ed Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Fri, 8 Feb 2019 15:02:22 +0100 Subject: [PATCH] refactored controller code. now going to test it --- config/default.yaml | 4 +++ script/controller.py | 83 +++++++++++++++++++++++++++----------------- script/imitator.py | 78 ++++++++++------------------------------- 3 files changed, 75 insertions(+), 90 deletions(-) diff --git a/config/default.yaml b/config/default.yaml index f98a100..aea6fb4 100644 --- a/config/default.yaml +++ b/config/default.yaml @@ -7,4 +7,8 @@ "lr": 0.6060229593671598, "cr": [-1.9731173515319824, -0.04246790334582329, -0.050492866697747864], "arm": 0.66392470071039278 + "should": { + "L": [0, 0.08, 0.15], + "R": [0, -0.08, 0.15] + } } diff --git a/script/controller.py b/script/controller.py index fb29d9b..232fc54 100755 --- a/script/controller.py +++ b/script/controller.py @@ -1,8 +1,9 @@ #! /usr/bin/env python """Controller should execute control for a given effector""" import os +import json +from math import asin, atan, radians -# import rospy import numpy as np from naoqi import ALProxy @@ -11,6 +12,15 @@ FRAME_TORSO = 0 K = 0.1 mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559) +_here = os.path.dirname(os.path.realpath(__file__)) +with open('{}/../config/default.yaml'.format(_here)) as f: + _xxx = json.load(f) + +ARM = _xxx['arm'] +MY_SHOULDERS = _xxx['should'] +JOINTS = ('ShoulderPitch', 'ShoulderRoll', 'ElbowYaw', 'ElbowRoll') +NAO_ARM = 0.22 + def get_transform(joint): return np.array( @@ -22,50 +32,61 @@ def xyz(joint): return np.array(mp.getPosition(joint, FRAME_TORSO, False))[:3] -def jacobian(): - # get current positions - # according to control figure these values should actually come - # from the integration step in the previous first control loop - - end_position = xyz('LArm') - shoulder_position = xyz('LShoulderPitch') - bicep_position = xyz('LShoulderRoll') - elbow_position = xyz('LElbowYaw') - forearm_position = xyz('LElbowRoll') - - # get transformed rotation axes, transformation to torso frame +def jacobian(side): + end_xyz = xyz('{}Arm'.format(side)) + xyzs = np.array([xyz(side + j) for j in JOINTS]) x_axis = np.array([[1, 0, 0, 1]]).T y_axis = np.array([[0, 1, 0, 1]]).T z_axis = np.array([[0, 0, 1, 1]]).T - shoulder_axis = get_transform('LShoulderPitch').dot(y_axis) - bicep_axis = get_transform('LShoulderRoll').dot(z_axis) - elbow_axis = get_transform('LElbowYaw').dot(x_axis) - forearm_axis = get_transform('LElbowRoll').dot(z_axis) - - xyzs = np.array([shoulder_position, bicep_position, elbow_position, - forearm_position]) + ax_order = (y_axis, z_axis, x_axis, z_axis) axes = np.concatenate( - [shoulder_axis, bicep_axis, elbow_axis, forearm_axis], + [get_transform(side + j).dot(ax) for j, ax in zip(JOINTS, ax_order)], axis=1 )[:3].T - jacobian = np.cross(axes, end_position - xyzs).T + jacobian = np.cross(axes, end_xyz - xyzs).T return jacobian -def pseudo_inverse(jacobian): - return np.linalg.pinv(jacobian) +def to_nao(my_xyz, side): + sh_offs = np.array(MY_SHOULDERS[side]) + my_sh_xyz = my_xyz - sh_offs + nao_sh_xyz = my_sh_xyz / ARM * NAO_ARM + nao_xyz = nao_sh_xyz + xyz(side + 'Shoulder') + return nao_xyz -def reference_generator(xyz): - delta = K * (xyz - xyz('LArm')) - return np.linalg.pinv(jacobian()).dot(delta).flatten() +def our_cartesian(my_xyz, side): + nao_xyz = to_nao(my_xyz, side) + delta_r = K * (nao_xyz - xyz('{}Arm'.format(side))) + crt_q = mp.getAngles([side + j for j in JOINTS], False) + delta_q = np.linalg.pinv(jacobian()).dot(delta_r).flatten() + return crt_q + delta_q -def movement(xyz): - ref = reference_generator(np.array(xyz)).tolist() - mp.changeAngles(['LShoulderPitch', 'LShoulderRoll', - 'LElbowYaw', 'LElbowRoll'], ref, 0.2) +def nao_cartesian(my_xyz, side): + pass + + +def _elbow(arm_): + quot = min(1.0, arm_ / ARM) + return radians(180 * (1 - quot)) + + +def dumb(my_xyz, side): + sign = 1 if side == 'L' else -1 + roll = asin(my_xyz[1] / np.linalg.norm(my_xyz)) + roll -= sign * radians(25) + pitch = atan(-my_xyz[2] / np.abs(my_xyz[0])) + + eroll = -sign * _elbow(np.linalg.norm(my_xyz)) + eyaw = mp.getAngles(['{}ElbowYaw'.format(side)], False)[0] + return np.array([pitch, roll, eyaw, eroll]) + + +def movement(my_xyz, side, control): + ref = control(np.array(my_xyz), side).tolist() + mp.setAngles([side + j for j in JOINTS], ref, 0.3) diff --git a/script/imitator.py b/script/imitator.py index acbf8eb..913a1a7 100755 --- a/script/imitator.py +++ b/script/imitator.py @@ -1,44 +1,30 @@ #! /usr/bin/env python import os -import json -from math import atan, asin, radians, sqrt import rospy import tf -import numpy as np from naoqi import ALProxy from masterloop import inform_masterloop_factory +from controller import movement, dumb, mp _inform_masterloop = inform_masterloop_factory('imitator') -_FRAME_TORSO = 0 -arm = None - - -def _elbow(arm_): - quot = min(1.0, arm_ / arm) - return radians(180 * (1 - quot)) +TORSO = False if __name__ == '__main__': rospy.init_node('imitator') rospy.wait_for_service('inform_masterloop') + ll = tf.TransformListener() + am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559) am.setExpressiveListeningEnabled(False) - mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559) + mp.wakeUp() - mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'], - [radians(-70), radians(70), -radians(40), radians(40)], 0.5) - - here = os.path.dirname(os.path.realpath(__file__)) - with open('{}/../config/default.yaml'.format(here)) as f: - x = json.load(f) - - arm = x['arm'] while not rospy.is_shutdown(): rospy.Rate(10).sleep() @@ -46,24 +32,20 @@ if __name__ == '__main__': continue rospy.logdebug('IMITATOR: ACTIVE') - # torso movement - - # try: - # _, q = ll.lookupTransform('odom', - # 'Aruco_0_frame', - # rospy.Time(0)) - # rot = tf.transformations.euler_from_quaternion(q) - # mp.setAngles(['LHipYawPitch', 'RHipYawPitch'], - # [-rot[1], -rot[1]], 0.3) - # except Exception as e: - # rospy.logwarn(e) - - # continue - - for i, eff in enumerate(['L', - 'R'], 1): + if TORSO: try: - trans, _ = ll.lookupTransform( + _, q = ll.lookupTransform('odom', + 'Aruco_0_frame', + rospy.Time(0)) + rot = tf.transformations.euler_from_quaternion(q) + mp.setAngles(['LHipYawPitch', 'RHipYawPitch'], + [-rot[1], -rot[1]], 0.3) + except Exception as e: + rospy.logwarn(e) + + for i, side in enumerate(['L', 'R'], 1): + try: + my_arm_xyz, _ = ll.lookupTransform( 'Aruco_0_frame', 'Aruco_{}_frame'.format(i), rospy.Time(0) @@ -72,28 +54,6 @@ if __name__ == '__main__': rospy.logwarn(e) continue - sign = 1 if eff == 'L' else -1 - roll = asin(trans[1] / - sqrt(trans[0]**2 + trans[1]**2 + trans[2]**2)) - roll -= sign * radians(25) - pitch = atan(-trans[2] / abs(trans[0])) - - eroll = -sign * _elbow(np.linalg.norm(np.array(trans))) - - mp.setAngles([ - '{}ShoulderRoll'.format(eff), - '{}ShoulderPitch'.format(eff), - '{}ElbowRoll'.format(eff) - ], [ - roll, - pitch, - eroll - ], 0.3) - - # targ = np.array(trans) - # targ = targ / np.linalg.norm(targ) * 0.3 - # mp.setPositions(eff, _FRAME_TORSO, - # targ.tolist() + [0, 0, 0], 0.5, 7) - # print eff, targ + movement(my_arm_xyz, side, dumb) mp.rest()