pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

did some cartesian control

Browse Source
This commit is contained in:
Pavel Lutskov
2019-02-04 17:55:55 +01:00
parent 36f787087c
commit c94ad77d06
3 changed files with 91 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

127
script/cartesian_controller.py
View File

@@ -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()

90
script/controller.py Executable file
View File

@@ -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)

3
script/imitator.py
View File

@@ -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')