diff --git a/config/default.json b/config/default.json
deleted file mode 100644
index 6bad3cd..0000000
--- a/config/default.json
+++ /dev/null
@@ -1,10 +0,0 @@
-{
- "cr": [-1.95, 0.00, 0.00],
- "fw": [-1.70, -1.40],
- "bk": [-2.20, -2.40],
- "lt": [ 0.35, 0.53],
- "rt": [-0.35, -0.53],
- "lr": [ 0.45, 0.85],
- "rr": [-0.45, -0.85],
- "arm": 70
-}
diff --git a/config/default.yaml b/config/default.yaml
index 496ca25..2545aaa 100644
--- a/config/default.yaml
+++ b/config/default.yaml
@@ -1,14 +1,24 @@
{
- "rt": -0.7155492901802063,
- "rr": -0.6855806884976677,
- "fw": -1.3561756610870361,
- "bk": -2.5843331813812256,
- "lt": 0.5666157603263855,
- "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]
- }
-}
+ "rt": -0.6568748354911804,
+ "lt": 0.5433389544487,
+ "rsh": [
+ 0.0,
+ -0.20736836269497871,
+ 0.08598418533802032
+ ],
+ "rr": -0.560604497907194,
+ "fw": -1.1799260377883911,
+ "cr": [
+ -1.9407565593719482,
+ -0.03346257656812668,
+ -0.08454087852040597
+ ],
+ "bk": -2.479292869567871,
+ "arm": 0.46978936571017016,
+ "lsh": [
+ 0.0,
+ 0.1996915601193905,
+ 0.09728588163852692
+ ],
+ "lr": 0.9522803428862967
+}
\ No newline at end of file
diff --git a/launch/calibration.launch b/launch/calibration.launch
index c786f91..4853c7a 100644
--- a/launch/calibration.launch
+++ b/launch/calibration.launch
@@ -8,5 +8,7 @@
output="screen"/>
-
+
+
diff --git a/launch/fulltest.launch b/launch/fulltest.launch
index 7770599..f94ed1f 100644
--- a/launch/fulltest.launch
+++ b/launch/fulltest.launch
@@ -15,14 +15,15 @@
output="screen"/>
+
+
-
+ -->
diff --git a/script/calibrator.py b/script/calibrator.py
index d2bd9a3..5eb5d57 100755
--- a/script/calibrator.py
+++ b/script/calibrator.py
@@ -1,7 +1,6 @@
#! /usr/bin/env python
import os
import json
-from sys import argv
import rospy
import tf
@@ -9,167 +8,127 @@ import actionlib
import numpy as np
from naoqi import ALProxy
-from masterloop import inform_masterloop_factory
from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
-_inform_masterloop = inform_masterloop_factory('calibrator')
+def calib_field(sentences, src_frames, targ_frames):
+ ok = False
+ t_and_q = []
+
+ while not ok:
+ for s in sentences:
+ tts.say(s)
+ tts.say('Then say start')
+ client.send_goal(RequestSpeechGoal(['start']))
+ client.wait_for_result()
+ for src, targ in zip(src_frames, targ_frames):
+ try:
+ t_and_q.append(ll.lookupTransform(src, targ, rospy.Time(0)))
+ ok = True
+ except:
+ ok = False
+ tts.say('Step failed. Try again')
+ break
+
+ return t_and_q
+
+
+def calib_center():
+ trans, q = calib_field([
+ 'Stand in front of camera',
+ 'Far enough to see your arms'
+ ], ['odom'], ['Aruco_0_frame'])[0]
+ rot = tf.transformations.euler_from_quaternion(q)
+ return trans, rot
+
+
+def calib_arms():
+ ts_and_qs = calib_field(
+ ['Now stretch your arms', 'Make sure the markers are detected'],
+ ['odom'] * 3,
+ ['Aruco_{}_frame'.format(i) for i in range(3)]
+ )
+ a0 = ts_and_qs[0][0]
+ a1 = ts_and_qs[1][0]
+ a2 = ts_and_qs[2][0]
+ return (
+ tuple(c1 - c0 for c0, c1 in zip(a0, a1)),
+ tuple(c2 - c0 for c0, c2 in zip(a0, a2))
+ )
+
+
+def calib_shoulders():
+ ts_and_qs = calib_field(
+ ['Now place the markers on the corresponding shoulders',
+ 'Make sure the markers are detected'],
+ ['odom'] * 3,
+ ['Aruco_{}_frame'.format(i) for i in range(3)]
+ )
+ a0 = ts_and_qs[0][0]
+ a1 = ts_and_qs[1][0]
+ a2 = ts_and_qs[2][0]
+ return (
+ tuple(c1 - c0 for c0, c1 in zip(a0, a1)),
+ tuple(c2 - c0 for c0, c2 in zip(a0, a2))
+ )
+
+
+def calib_rotation():
+ rots = []
+ for side in ('left', 'right'):
+ _, q = calib_field(
+ ['Now rotate to your {}'.format(side),
+ 'So that marker on the chest is still detected'],
+ ['odom'], ['Aruco_0_frame']
+ )[0]
+ rots.append(tf.transformations.euler_from_quaternion(q))
+ return tuple(rots)
+
+
+def calib_extremes():
+ transs = []
+ for direction in ('forward', 'backward', 'to the left', 'to the right'):
+ trans, _ = calib_field(
+ ['Now return to the initial position',
+ 'Take a big step {}'.format(direction)],
+ ['odom'],
+ ['Aruco_0_frame']
+ )[0]
+ transs.append(trans)
+
+ return tuple(transs)
def calibration():
- tts.say('Stand in front of camera')
- tts.say('Far enough to see your arms')
- tts.say('Then say start')
- client.send_goal(RequestSpeechGoal(['start']))
- client.wait_for_result()
+ center = calib_center()
+ larm, rarm = calib_arms()
+ lsh, rsh = calib_shoulders()
+ larm, rarm, lsh, rsh = larm[1:], rarm[1:], lsh[1:], rsh[1:]
+ lr, rr = calib_rotation()
+ fw, bk, lt, rt = calib_extremes()
- try:
- trans, q = ll.lookupTransform('odom',
- 'Aruco_0_frame',
- rospy.Time(0))
- rot = tf.transformations.euler_from_quaternion(q)
- center = trans, rot
+ tts.say('Well done')
- except Exception:
- pass
+ calib = {
+ 'cr': [center[0][0], center[0][1], center[1][2]],
+ 'arm': (np.linalg.norm(np.array(larm) - np.array(lsh)) +
+ np.linalg.norm(np.array(rarm) - np.array(rsh))) / 2,
+ 'lsh': [0.0] + list(lsh),
+ 'rsh': [0.0] + list(rsh),
+ 'rr': rr[2],
+ 'lr': lr[2],
+ 'fw': fw[0],
+ 'bk': bk[0],
+ 'lt': lt[1],
+ 'rt': rt[1],
+ }
- tts.say('Now stretch your arms')
- tts.say('Make sure the markers are detected')
- tts.say('Then say start')
- client.send_goal(RequestSpeechGoal(['start']))
- client.wait_for_result()
-
- try:
- larm, _ = ll.lookupTransform('Aruco_0_frame',
- 'Aruco_1_frame',
- rospy.Time(0))
- rarm, _ = ll.lookupTransform('Aruco_0_frame',
- 'Aruco_2_frame',
- rospy.Time(0))
- except Exception:
- pass
-
- tts.say('Now place the markers on the corresponding shoulders')
- tts.say('Make sure the markers are detected')
- tts.say('Then say start')
- client.send_goal(RequestSpeechGoal(['start']))
- client.wait_for_result()
-
- try:
- lsh, _ = ll.lookupTransform('Aruco_0_frame',
- 'Aruco_1_frame',
- rospy.Time(0))
- rsh, _ = ll.lookupTransform('Aruco_0_frame',
- 'Aruco_2_frame',
- rospy.Time(0))
- except Exception:
- pass
-
- tts.say('Now rotate to your right')
- tts.say('So that marker is still detected')
- tts.say('Then say start')
- client.send_goal(RequestSpeechGoal(['start']))
- client.wait_for_result()
-
- try:
- _, q = ll.lookupTransform('odom',
- 'Aruco_0_frame',
- rospy.Time(0))
- RR = tf.transformations.euler_from_quaternion(q)
- except Exception:
- pass
-
- tts.say('Now rotate to your left')
- tts.say('So that marker is still detected')
- tts.say('Then say start')
- client.send_goal(RequestSpeechGoal(['start']))
- client.wait_for_result()
-
- try:
- _, q = ll.lookupTransform('odom',
- 'Aruco_0_frame',
- rospy.Time(0))
- LR = tf.transformations.euler_from_quaternion(q)
- except Exception:
- pass
-
-
- tts.say('Now rotate to initial position')
- tts.say('Take a big step forward')
- tts.say('Then say start')
- client.send_goal(RequestSpeechGoal(['start']))
- client.wait_for_result()
-
- try:
- FW, _ = ll.lookupTransform('odom',
- 'Aruco_0_frame',
- rospy.Time(0))
- except Exception:
- pass
-
- tts.say('Now return to the initial position')
- tts.say('Then take a big step backward')
- tts.say('Then say start')
- client.send_goal(RequestSpeechGoal(['start']))
- client.wait_for_result()
-
- try:
- BK, _ = ll.lookupTransform('odom',
- 'Aruco_0_frame',
- rospy.Time(0))
- except Exception:
- pass
-
- tts.say('Now return to the initial position')
- tts.say('Then take a big step to the right')
- tts.say('Then say start')
- client.send_goal(RequestSpeechGoal(['start']))
- client.wait_for_result()
-
- try:
- RT, _ = ll.lookupTransform('odom',
- 'Aruco_0_frame',
- rospy.Time(0))
- except Exception:
- pass
-
- tts.say('Now return to the initial position')
- tts.say('Then take a big step to the left')
- tts.say('Then say start')
- client.send_goal(RequestSpeechGoal(['start']))
- client.wait_for_result()
-
- try:
- LT, _ = ll.lookupTransform('odom',
- 'Aruco_0_frame',
- rospy.Time(0))
- except Exception:
- pass
-
- calib = {}
-
- try:
- tts.say('Well done')
- calib['cr'] = [center[0][0], center[0][1], center[1][2]]
- calib['arm'] = (np.linalg.norm(np.array(larm)) +
- np.linalg.norm(np.array(rarm))) / 2
- calib['rr'] = RR[2]
- calib['lr'] = LR[2]
- calib['fw'] = FW[0]
- calib['bk'] = BK[0]
- calib['lt'] = LT[1]
- calib['rt'] = RT[1]
- here = os.path.dirname(os.path.realpath(__file__))
- with open('{}/../config/default.yaml'.format(here), 'w') as f:
- json.dump(calib, f)
-
- except:
- tts.say('Something has gone very wrong')
+ here = os.path.dirname(os.path.realpath(__file__))
+ with open('{}/../config/default.yaml'.format(here), 'w') as f:
+ json.dump(calib, f, indent=4)
if __name__ == '__main__':
- if len(argv) < 2:
- raise SystemExit
rospy.init_node('calibrator', disable_signals=True)
@@ -184,5 +143,6 @@ if __name__ == '__main__':
rospy.on_shutdown(lambda: client.cancel_goal())
calibration()
+
except KeyboardInterrupt:
rospy.signal_shutdown('Interrupted')
diff --git a/script/controller.py b/script/controller.py
index 302e233..9d4f9cd 100755
--- a/script/controller.py
+++ b/script/controller.py
@@ -1,5 +1,6 @@
#! /usr/bin/env python
"""Controller should execute control for a given effector"""
+from __future__ import division
import os
import json
from math import asin, atan, radians
@@ -10,6 +11,7 @@ from masterloop import mp
FRAME_TORSO = 0
+AXIS_XYZ = 7
K = 0.1
_here = os.path.dirname(os.path.realpath(__file__))
@@ -17,7 +19,10 @@ with open('{}/../config/default.yaml'.format(_here)) as f:
_xxx = json.load(f)
ARM = _xxx['arm']
-MY_SHOULDERS = _xxx['should']
+MY_SHOULDERS = {
+ 'L': _xxx['lsh'],
+ 'R': _xxx['rsh']
+}
JOINTS = ('ShoulderPitch', 'ShoulderRoll', 'ElbowYaw', 'ElbowRoll')
NAO_ARM = 0.22
@@ -59,20 +64,24 @@ def to_nao(my_xyz, side):
return nao_xyz
+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)
+ s_inv[np.abs(s_inv) > 30] = 0
+ return vt.T.dot(s_inv).dot(u.T)
+
+
def our_cartesian(my_xyz, side):
nao_xyz = to_nao(my_xyz, side)
- delta_r = K * (nao_xyz - xyz('{}Arm'.format(side)))
+ delta_r = 0.1 * (nao_xyz - xyz('{}Arm'.format(side)))
crt_q = mp.getAngles([side + j for j in JOINTS], False)
- delta_q = np.linalg.pinv(jacobian(side)).dot(delta_r).flatten()
+ delta_q = inv_jacobian(jacobian(side)).dot(delta_r).flatten()
return crt_q + delta_q
-def nao_cartesian(my_xyz, side):
- pass
-
-
def _elbow(arm_):
- quot = min(1.0, arm_ / ARM)
+ quot = min(1.0, arm_ / 0.70)
return radians(180 * (1 - quot))
@@ -83,10 +92,17 @@ def dumb(my_xyz, side):
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]
+ eyaw = -sign * radians(40)
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)
+
+
+def nao_cart_movement(my_arm_xyz, side, *_):
+ nao_xyz = to_nao(my_arm_xyz, side)
+ mp.setPositions('{}Arm'.format(side), FRAME_TORSO,
+ tuple(nao_xyz.tolist()) + (0, 0, 0),
+ 1.0, AXIS_XYZ)
diff --git a/script/imitator.py b/script/imitator.py
index fce11aa..7c5f3a5 100755
--- a/script/imitator.py
+++ b/script/imitator.py
@@ -1,10 +1,12 @@
#! /usr/bin/env python
+from argparse import ArgumentParser
import rospy
import tf
+import numpy as np
from masterloop import inform_masterloop_factory
-from controller import movement, dumb, our_cartesian
+from controller import nao_cart_movement, movement, dumb, our_cartesian
_inform_masterloop = inform_masterloop_factory('imitator')
@@ -20,7 +22,7 @@ if __name__ == '__main__':
ll = tf.TransformListener()
while not rospy.is_shutdown():
- rospy.Rate(10).sleep()
+ rospy.Rate(20).sleep()
if not _inform_masterloop('imitate'):
continue
rospy.logdebug('IMITATOR: ACTIVE')
@@ -38,13 +40,20 @@ if __name__ == '__main__':
for i, side in enumerate(['L', 'R'], 1):
try:
- my_arm_xyz, _ = ll.lookupTransform(
+ a0, _ = ll.lookupTransform(
+ 'odom',
'Aruco_0_frame',
+ rospy.Time(0)
+ )
+ arm, _ = ll.lookupTransform(
+ 'odom',
'Aruco_{}_frame'.format(i),
rospy.Time(0)
)
except Exception as e:
rospy.logwarn(e)
continue
-
- movement(my_arm_xyz, side, our_cartesian)
+ my_arm_xyz = np.array(arm) - np.array(a0)
+ # rospy.loginfo('{}'.format(my_arm_xyz))
+ # rospy.loginfo('{}'.format(dumb(my_arm_xyz, side)))
+ movement(my_arm_xyz, side, dumb)
diff --git a/script/masterloop.py b/script/masterloop.py
index bb336bf..26ea8c6 100755
--- a/script/masterloop.py
+++ b/script/masterloop.py
@@ -172,7 +172,8 @@ if __name__ == '__main__':
if not speech_in_progress:
speech_in_progress = True
speech.send_goal(RequestSpeechGoal(
- VOC_STATE[state] + VOC_HAND[hand_state]
+ VOC_STATE[state] +
+ (VOC_HAND[hand_state] if state != 'dead' else [])
), speech_done_cb)
else:
if speech_in_progress: