From c94ad77d06b1b231200c2ff58374859492697ab6 Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Mon, 4 Feb 2019 17:55:55 +0100 Subject: [PATCH] did some cartesian control --- script/cartesian_controller.py | 127 --------------------------------- script/controller.py | 90 +++++++++++++++++++++++ script/imitator.py | 3 +- 3 files changed, 91 insertions(+), 129 deletions(-) delete mode 100755 script/cartesian_controller.py create mode 100755 script/controller.py diff --git a/script/cartesian_controller.py b/script/cartesian_controller.py deleted file mode 100755 index 6988774..0000000 --- a/script/cartesian_controller.py +++ /dev/null @@ -1,127 +0,0 @@ -#! /usr/bin/env python -import os - -import rospy -import numpy as np - -from naoqi import ALProxy -import motion - - -mp = None - - -def get_transform(joint): - frame = motion.FRAME_TORSO - useSensorValues = True - result = mp.getTransform(joint,frame,useSensorValues) - result = np.matrix(result) - print result - result = np.reshape(result, (4,4)) - print result - return result - - -def cartesian_position(joint): - print 'function' - frame = motion.FRAME_TORSO - useSensorValues = True - result = mp.getPosition(joint, frame, useSensorValues) - #print result - return np.array(result[: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 = cartesian_position('LArm') - - shoulder_position = cartesian_position('LShoulderPitch') - bicep_position = cartesian_position('LShoulderRoll') - elbow_position = cartesian_position('LElbowYaw') - forearm_position = cartesian_position('LElbowRoll') - - # get transformed rotation axes, transformation to torso frame - - 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) - - # get basis vectors of jacobian - - shoulder_basis = np.cross(shoulder_axis[:3].flatten(), - end_position - shoulder_position) - bicep_basis = np.cross(bicep_axis[:3].flatten(), - end_position - bicep_position) - elbow_basis = np.cross(elbow_axis[:3].flatten(), - end_position - elbow_position) - forearm_basis = np.cross(forearm_axis[:3].flatten(), - end_position - forearm_position) - - # build jacobian matrix - jacobian = np.concatenate( - [shoulder_basis, bicep_basis, elbow_basis, forearm_basis], - axis=0 - ).T - - return jacobian - - -def pseudo_inverse(jacobian): - return np.linalg.pinv(jacobian) - - -def reference_generator(p_d): - - # calculate jacobian - - # use jacobian to compute desired joint speed - - # integrate over desired speed to get desired joint position - - # return desired joint position and speed - - return - - -def movement(e): - - # scale joint states with matrix K - - # add desired joint speed - - # move robot arm - - return - - -if __name__ == '__main__': - mp = ALProxy("ALMotion", os.environ['NAO_IP'], 9559) - jacob = jacobian() - print jacob - jacob = pseudo_inverse(jacob) - print(jacob) - - # given new desired coordinates - - e = 1; - """ - while e bigger some value - - # run reference generator to get desired joint postion and speed - - # subtract current joint states - - # movement - - """ - #rospy.init_node('cartesian_masterloop') - #rospy.spin() - diff --git a/script/controller.py b/script/controller.py new file mode 100755 index 0000000..35947bd --- /dev/null +++ b/script/controller.py @@ -0,0 +1,90 @@ +#! /usr/bin/env python +"""Controller should execute control for a given effector""" +import os + +# import rospy +import numpy as np +from naoqi import ALProxy + + +FRAME_TORSO = 0 +K = 0.1 +mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559) +mp.wakeUp() + + +def _get_transform(joint): + return np.array( + mp.getTransform(joint, FRAME_TORSO, False) + ).reshape((4, 4)) + + +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 + + 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]) + + axes = np.concatenate( + [shoulder_axis, bicep_axis, elbow_axis, forearm_axis], + axis=1 + )[:3].T + + # get basis vectors of jacobian + + jacobian = np.cross(axes, end_position - xyzs).T + + # shoulder_basis = np.cross(shoulder_axis[:3].flatten(), + # end_position - shoulder_position) + # bicep_basis = np.cross(bicep_axis[:3].flatten(), + # end_position - bicep_position) + # elbow_basis = np.cross(elbow_axis[:3].flatten(), + # end_position - elbow_position) + # forearm_basis = np.cross(forearm_axis[:3].flatten(), + # end_position - forearm_position) + + # build jacobian matrix + # jacobian = np.concatenate( + # [shoulder_basis, bicep_basis, elbow_basis, forearm_basis], + # axis=0 + # ).T + + return jacobian + + +def pseudo_inverse(jacobian): + return np.linalg.pinv(jacobian) + + +def reference_generator(xyz): + delta = K * (xyz - _xyz('LArm')) + return np.linalg.pinv(jacobian()).dot(delta).flatten() + + +def movement(xyz): + ref = reference_generator(np.array(xyz)).tolist() + mp.changeAngles(['LShoulderPitch', 'LShoulderRoll', + 'LElbowYaw', 'LElbowRoll'], ref, 0.2) diff --git a/script/imitator.py b/script/imitator.py index f525c01..acbf8eb 100755 --- a/script/imitator.py +++ b/script/imitator.py @@ -1,7 +1,6 @@ #! /usr/bin/env python import os import json -from time import sleep from math import atan, asin, radians, sqrt import rospy @@ -42,7 +41,7 @@ if __name__ == '__main__': arm = x['arm'] while not rospy.is_shutdown(): - sleep(0.1) + rospy.Rate(10).sleep() if not _inform_masterloop('imitate'): continue rospy.logdebug('IMITATOR: ACTIVE')