diff --git a/config/default.yaml b/config/default.yaml index 2545aaa..36f8755 100644 --- a/config/default.yaml +++ b/config/default.yaml @@ -1,24 +1,24 @@ { - "rt": -0.6568748354911804, - "lt": 0.5433389544487, + "rt": -0.6346718668937683, + "lt": 0.4780789315700531, "rsh": [ 0.0, - -0.20736836269497871, - 0.08598418533802032 + -0.20951810479164124, + 0.11152809858322144 ], - "rr": -0.560604497907194, - "fw": -1.1799260377883911, + "rr": -0.9144995069967937, + "fw": -1.158365249633789, "cr": [ - -1.9407565593719482, - -0.03346257656812668, - -0.08454087852040597 + -1.8029232025146484, + 8.554260421078652e-05, + -0.014778438399650092 ], - "bk": -2.479292869567871, - "arm": 0.46978936571017016, + "bk": -2.5150508880615234, + "arm": 0.48753388244912577, "lsh": [ 0.0, - 0.1996915601193905, - 0.09728588163852692 + 0.18295197188854218, + 0.11881899833679199 ], - "lr": 0.9522803428862967 + "lr": 0.9070798867837204 } \ No newline at end of file diff --git a/launch/with_gui.launch b/launch/with_gui.launch index 9b92791..e9f6c48 100644 --- a/launch/with_gui.launch +++ b/launch/with_gui.launch @@ -4,6 +4,10 @@ + + diff --git a/script/controller.py b/script/controller.py index 9d4f9cd..910e1ad 100755 --- a/script/controller.py +++ b/script/controller.py @@ -27,14 +27,51 @@ JOINTS = ('ShoulderPitch', 'ShoulderRoll', 'ElbowYaw', 'ElbowRoll') NAO_ARM = 0.22 +def xyz(joint): + return np.array(mp.getPosition(joint, FRAME_TORSO, False), + dtype=np.float64)[:3] + + +crt_xyz = { + 'L': xyz('LArm'), + 'R': xyz('RArm') +} + + +crt_joint = { + 'L': np.array(mp.getAngles(['L' + j for j in JOINTS], False), + dtype=np.float64), + 'R': np.array(mp.getAngles(['R' + j for j in JOINTS], False), + dtype=np.float64) +} + + +best_guess = { + 'L': np.random.randn(3, 4), + 'R': np.random.randn(3, 4) +} + + def get_transform(joint): return np.array( - mp.getTransform(joint, FRAME_TORSO, False) + mp.getTransform(joint, FRAME_TORSO, False), dtype=np.float64 ).reshape((4, 4)) -def xyz(joint): - return np.array(mp.getPosition(joint, FRAME_TORSO, False))[:3] +def diff_jacobian(side): + new_end_xyz = xyz('{}Arm'.format(side)) + delta_r = new_end_xyz - crt_xyz[side] + crt_xyz[side] = new_end_xyz.copy() + + new_joint = np.array(mp.getAngles([side + j for j in JOINTS], False)) + delta_j = new_joint - crt_joint[side] + crt_joint[side] = new_joint.copy() + + J = delta_r[:,None] / delta_j + okval = np.isfinite(J) + J[~okval] = best_guess[side][~okval] + best_guess[side][okval] = J[okval] + return J def jacobian(side): @@ -52,8 +89,8 @@ def jacobian(side): axis=1 )[:3].T - jacobian = np.cross(axes, end_xyz - xyzs).T - return jacobian + J = np.cross(axes, end_xyz - xyzs).T + return J def to_nao(my_xyz, side): @@ -67,19 +104,27 @@ def to_nao(my_xyz, side): def inv_jacobian(j): u, s, vt = np.linalg.svd(j) s_inv = np.zeros((vt.shape[0], u.shape[1])) - np.fill_diagonal(s_inv, 1 // s) + np.fill_diagonal(s_inv, 1 / s) s_inv[np.abs(s_inv) > 30] = 0 return vt.T.dot(s_inv).dot(u.T) -def our_cartesian(my_xyz, side): +def _some_cartesian(my_xyz, side, jfunc): nao_xyz = to_nao(my_xyz, side) delta_r = 0.1 * (nao_xyz - xyz('{}Arm'.format(side))) crt_q = mp.getAngles([side + j for j in JOINTS], False) - delta_q = inv_jacobian(jacobian(side)).dot(delta_r).flatten() + delta_q = inv_jacobian(jfunc(side)).dot(delta_r).flatten() return crt_q + delta_q +def our_cartesian(my_xyz, side): + return _some_cartesian(my_xyz, side, jacobian) + + +def our_diff_cartesian(my_xyz, side): + return _some_cartesian(my_xyz, side, diff_jacobian) + + def _elbow(arm_): quot = min(1.0, arm_ / 0.70) return radians(180 * (1 - quot)) diff --git a/script/imitator.py b/script/imitator.py index 9edda1c..02a8193 100755 --- a/script/imitator.py +++ b/script/imitator.py @@ -1,12 +1,14 @@ #! /usr/bin/env python from argparse import ArgumentParser +from math import radians import rospy import tf import numpy as np -from masterloop import inform_masterloop_factory -from controller import nao_cart_movement, movement, dumb, our_cartesian +from masterloop import inform_masterloop_factory, mp +from controller import nao_cart_movement, movement +from controller import dumb, our_cartesian, our_diff_cartesian _inform_masterloop = inform_masterloop_factory('imitator') @@ -23,6 +25,8 @@ def controller_factory(ctrl): cfunc = dumb elif ctrl == 'our_cartesian': cfunc = our_cartesian + elif ctrl == 'our_diff_cartesian': + cfunc = our_diff_cartesian return lambda my_arm_xyz, side: movement(my_arm_xyz, side, cfunc) @@ -30,7 +34,8 @@ if __name__ == '__main__': rospy.init_node('imitator') ap = ArgumentParser() ap.add_argument('--controller', default='dumb', const='dumb', nargs='?', - choices=['dumb', 'nao_cartesian', 'our_cartesian'], + choices=['dumb', 'nao_cartesian', 'our_cartesian', + 'our_diff_cartesian'], help='Choose the controller for arm motions') args, _ = ap.parse_known_args() imitation_cycle = controller_factory(args.controller) @@ -75,3 +80,5 @@ if __name__ == '__main__': # rospy.loginfo('{}'.format(my_arm_xyz)) # rospy.loginfo('{}'.format(dumb(my_arm_xyz, side))) imitation_cycle(my_arm_xyz, side) + mp.setAngles(('LWristYaw', 'RWristYaw'), + (radians(-70), radians(70)), 0.3) diff --git a/script/speech_server.py b/script/speech_server.py index 02c922d..1d695ee 100755 --- a/script/speech_server.py +++ b/script/speech_server.py @@ -49,7 +49,7 @@ class SpeechDetectorModule(ALModule): self._busy = False def get_status(self): - print(almem.getData('ALSpeechRecognition/Status')) + return almem.getData('ALSpeechRecognition/Status') def start_speech(self, voc, resume=False): if self._busy != resume: @@ -80,7 +80,6 @@ class SpeechDetectorModule(ALModule): def on_word_recognized(self, *_args): word, conf = almem.getData('WordRecognized') - print(word, conf) if conf > 0.4: self.stop_speech(pause=True) self.tts.say(word) diff --git a/script/walker.py b/script/walker.py index fbd3358..df21d54 100755 --- a/script/walker.py +++ b/script/walker.py @@ -1,4 +1,6 @@ #! /usr/bin/env python +from __future__ import division + import os import json from time import sleep @@ -60,6 +62,7 @@ if __name__ == '__main__': rospy.wait_for_service('inform_masterloop') ll = tf.TransformListener() mp.setMoveArmsEnabled(False, False) + mp.setAngles(('HeadYaw', 'HeadPitch'), (0.0, 0.35), 0.3) while not rospy.is_shutdown(): sleep(0.3) @@ -101,6 +104,7 @@ if __name__ == '__main__': if not permission: mp.stopMove() else: + mp.setAngles(('HeadYaw', 'HeadPitch'), (0.0, 0.35), 0.3) mp.moveToward(*movement) # DON'T DELETE THIS ONE pass diff --git a/src/rviz_human.cpp b/src/rviz_human.cpp index 8df3062..ace96c9 100644 --- a/src/rviz_human.cpp +++ b/src/rviz_human.cpp @@ -65,8 +65,11 @@ int main( int argc, char** argv ) base_position_out.y = center[1]; base_position_out.z = torso.z-0.75*size; - double base_in_r = (config["fw"].as() - center[0])*0.33; - double base_out_r = config["fw"].as() - center[0]; + double base_in_r_x = (config["fw"].as() - center[0])*0.33; + double base_out_r_x = config["fw"].as() - center[0]; + + double base_in_r_y = (config["rt"].as() - center[1])*0.33; + double base_out_r_y = (config["rt"].as() - center[1]); @@ -377,12 +380,12 @@ int main( int argc, char** argv ) camera.scale.y = 0.2; camera.scale.z = 0.2; - base_in.scale.x = 2*base_in_r; - base_in.scale.y = 2*base_in_r; + base_in.scale.x = 2*base_in_r_x; + base_in.scale.y = 2*base_in_r_y; base_in.scale.z = 0.1; - base_out.scale.x = 2*base_out_r; - base_out.scale.y = 2*base_out_r; + base_out.scale.x = 2*base_out_r_x; + base_out.scale.y = 2*base_out_r_y; base_out.scale.z = 0.01; // Set the color