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

renamed controller to masterloop

Browse Source
This commit is contained in:
Pavel Lutskov
2019-02-04 16:23:48 +01:00
parent 3708633328
commit 36f787087c
9 changed files with 64 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
script/calibrator.py
View File

@@ -9,11 +9,11 @@ import actionlib
import numpy as np
from naoqi import ALProxy
from controller import inform_controller_factory
from masterloop import inform_masterloop_factory
from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
_inform_controller = inform_controller_factory('calibrator')
_inform_masterloop = inform_masterloop_factory('calibrator')
def calibration():

57
script/cartesian_controller.py
View File

@@ -1,19 +1,20 @@
#! /usr/bin/env python
import os
import os
import rospy
import numpy as np
import sys
from naoqi import ALProxy
import motion
motionProxy = 0
mp = None
def get_transform(joint):
frame = motion.FRAME_TORSO
useSensorValues = True
result = motionProxy.getTransform(joint,frame,useSensorValues)
result = mp.getTransform(joint,frame,useSensorValues)
result = np.matrix(result)
print result
result = np.reshape(result, (4,4))
@@ -25,15 +26,15 @@ def cartesian_position(joint):
print 'function'
frame = motion.FRAME_TORSO
useSensorValues = True
result = motionProxy.getPosition(joint, frame, useSensorValues)
result = mp.getPosition(joint, frame, useSensorValues)
#print result
return np.array(result[:3])
def jacobian():
# get current positions/ accordint to control figure these values should actually come from the
# integration step in the previous first control loop
# 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')
@@ -47,21 +48,28 @@ def jacobian():
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)
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
jacobian = np.concatenate(
[shoulder_basis, bicep_basis, elbow_basis, forearm_basis],
axis=0
).T
return jacobian
@@ -70,7 +78,7 @@ def pseudo_inverse(jacobian):
return np.linalg.pinv(jacobian)
def reference_generator(p_d)
def reference_generator(p_d):
# calculate jacobian
@@ -83,20 +91,19 @@ def reference_generator(p_d)
return
def movement(e)
def movement(e):
# scale joint states with matrix K
# add desired joint speed
# move robot arm
# move robot arm
return
if __name__ == '__main__':
motionProxy = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
mp = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
jacob = jacobian()
print jacob
jacob = pseudo_inverse(jacob)
@@ -105,7 +112,7 @@ if __name__ == '__main__':
# given new desired coordinates
e = 1;
"""
"""
while e bigger some value
# run reference generator to get desired joint postion and speed
@@ -113,8 +120,8 @@ if __name__ == '__main__':
# subtract current joint states
# movement
"""
#rospy.init_node('cartesian_controller')
"""
#rospy.init_node('cartesian_masterloop')
#rospy.spin()

12
script/fall_detector.py
View File

@@ -4,14 +4,14 @@ import os
import rospy
from naoqi import ALProxy, ALBroker, ALModule
from controller import inform_controller_factory
from masterloop import inform_masterloop_factory
fall_broker = None
almem = None
_inform_controller = inform_controller_factory('fall_detector')
_inform_masterloop = inform_masterloop_factory('fall_detector')
class FallDetectorModule(ALModule):
@@ -21,10 +21,10 @@ class FallDetectorModule(ALModule):
self.mp = ALProxy('ALMotion')
def on_robot_falling(self, *_args):
_inform_controller('falling')
_inform_masterloop('falling')
def on_robot_fallen(self, *_args):
if not _inform_controller('fallen'):
if not _inform_masterloop('fallen'):
return
self.mp.rest()
rospy.Rate(0.5).sleep()
@@ -33,12 +33,12 @@ class FallDetectorModule(ALModule):
while not pp.goToPosture('StandInit', 1.0):
pass
rospy.Rate(1).sleep()
_inform_controller('recovered')
_inform_masterloop('recovered')
if __name__ == "__main__":
rospy.init_node('fall_detector')
rospy.wait_for_service('inform_controller')
rospy.wait_for_service('inform_masterloop')
fall_broker = ALBroker("fall_broker", "0.0.0.0", 0,
os.environ['NAO_IP'], 9559)
fall_detector = FallDetectorModule("fall_detector")

8
script/imitator.py
View File

@@ -9,10 +9,10 @@ import tf
import numpy as np
from naoqi import ALProxy
from controller import inform_controller_factory
from masterloop import inform_masterloop_factory
_inform_controller = inform_controller_factory('imitator')
_inform_masterloop = inform_masterloop_factory('imitator')
_FRAME_TORSO = 0
@@ -26,7 +26,7 @@ def _elbow(arm_):
if __name__ == '__main__':
rospy.init_node('imitator')
rospy.wait_for_service('inform_controller')
rospy.wait_for_service('inform_masterloop')
ll = tf.TransformListener()
am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
am.setExpressiveListeningEnabled(False)
@@ -43,7 +43,7 @@ if __name__ == '__main__':
while not rospy.is_shutdown():
sleep(0.1)
if not _inform_controller('imitate'):
if not _inform_masterloop('imitate'):
continue
rospy.logdebug('IMITATOR: ACTIVE')

22
script/controller.py → script/masterloop.py
View File

@@ -4,7 +4,7 @@ from argparse import ArgumentParser
import rospy
import actionlib
from teleoperation.srv import InformController, InformControllerResponse
from teleoperation.srv import InformMasterloop, InformMasterloopResponse
from teleoperation.srv import Hands
from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
@@ -79,17 +79,17 @@ def speech_done_cb(_, result):
speech_in_progress = False
def inform_controller_factory(who):
def inform_controller(what):
def inform_masterloop_factory(who):
def inform_masterloop(what):
try:
inform_controller = rospy.ServiceProxy('inform_controller',
InformController)
perm = inform_controller(who, what).permission
inform_masterloop = rospy.ServiceProxy('inform_masterloop',
InformMasterloop)
perm = inform_masterloop(who, what).permission
except rospy.service.ServiceException:
rospy.signal_shutdown('Controller is dead')
rospy.signal_shutdown('Masterloop is dead')
perm = False
return perm
return inform_controller
return inform_masterloop
def handle_request(r):
@@ -129,12 +129,12 @@ def handle_request(r):
)
if _state_old != state:
rospy.loginfo('{} -> {}'.format(_state_old, state))
return InformControllerResponse(permission)
return InformMasterloopResponse(permission)
if __name__ == '__main__':
rospy.init_node('controller')
rospy.init_node('masterloop')
ap = ArgumentParser()
ap.add_argument('-i', '--autoimitate',
help='Switch between moving and imitating automatically',
@@ -144,7 +144,7 @@ if __name__ == '__main__':
AI = args.autoimitate
init_voc_state_speech()
ic = rospy.Service('inform_controller', InformController, handle_request)
ic = rospy.Service('inform_masterloop', InformMasterloop, handle_request)
speech = actionlib.SimpleActionClient('speech_server',
RequestSpeechAction)

12
script/walker.py
View File

@@ -7,7 +7,7 @@ import rospy
import tf
from naoqi import ALProxy
from controller import inform_controller_factory
from masterloop import inform_masterloop_factory
FW = None
@@ -40,7 +40,7 @@ def global_init():
RR = n_way(cz, x['rr'], 2), x['rr']
_inform_controller = inform_controller_factory('walker')
_inform_masterloop = inform_masterloop_factory('walker')
def _speed(pos, interval):
@@ -58,7 +58,7 @@ def _speed(pos, interval):
if __name__ == '__main__':
rospy.init_node('walker')
global_init()
rospy.wait_for_service('inform_controller')
rospy.wait_for_service('inform_masterloop')
ll = tf.TransformListener()
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
mp.wakeUp()
@@ -75,7 +75,7 @@ if __name__ == '__main__':
except Exception as e:
mp.stopMove()
_inform_controller('stop')
_inform_masterloop('stop')
continue
movement = [0, 0, 0]
@@ -95,12 +95,12 @@ if __name__ == '__main__':
if not any(movement):
rospy.logdebug('WALKER: STOP')
_inform_controller('stop')
_inform_masterloop('stop')
# mp.move(0, 0, 0)
mp.stopMove()
continue
permission = _inform_controller('move')
permission = _inform_masterloop('move')
if not permission:
mp.stopMove()