refactored controller code. now going to test it
This commit is contained in:
Pavel Lutskov
@@ -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]
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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()
|
||||
|
||||
Reference in New Issue
Block a user