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

implemented torso imitation (DANGEROUS!!!)

Browse Source 
also fixed usb_cam launch bug
This commit is contained in:
Pavel Lutskov
2019-01-30 11:33:13 +01:00
parent b95417293b
commit f050c3464a
3 changed files with 22 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
launch/fulltest.launch
View File

@@ -4,19 +4,17 @@
<launch> <launch>
<include file="$(find nao_apps)/launch/speech.launch"/> <include file="$(find nao_apps)/launch/speech.launch"/>
<!-- <node name="speech" pkg="teleoperation" type="speech" output="screen"/>
<node name="usb_cam_node" pkg="usb_cam" type="usb_cam_node"/> <node name="usb_cam" pkg="usb_cam" type="usb_cam_node"/>
--> <node name="aruco_detector" pkg="teleoperation" type="aruco_detector"
output="screen"/>
<node name="controller" pkg="teleoperation" type="controller.py" <node name="controller" pkg="teleoperation" type="controller.py"
output="screen"/> output="screen"/>
<node name="speech" pkg="teleoperation" type="speech" output="screen"/> <node name="imitator" pkg="teleoperation" type="imitator.py"
<node name="aruco_detector" pkg="teleoperation" type="aruco_detector"
output="screen"/> output="screen"/>
<!-- <!--
<node name="walker" pkg="teleoperation" type="walker.py"/> <node name="walker" pkg="teleoperation" type="walker.py"/>
-->
<node name="imitator" pkg="teleoperation" type="imitator.py"
output="screen"/>
<node name="fall_detector" pkg="teleoperation" type="fall_detector.py"/> <node name="fall_detector" pkg="teleoperation" type="fall_detector.py"/>
-->
</launch> </launch>

15
script/imitator.py
View File

@@ -30,6 +30,21 @@ if __name__ == '__main__':
if not _inform_controller('imitate'): if not _inform_controller('imitate'):
continue continue
rospy.logdebug('IMITATOR: ACTIVE') 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', for i, eff in enumerate(['L',
'R'], 1): 'R'], 1):
try: try:

6
script/walker.py
View File

@@ -10,7 +10,7 @@ from controller import inform_controller_factory
#min #max #min #max
FW = -1.65, -1.45 FW = -1.70, -1.40
BK = -2.20, -2.40 BK = -2.20, -2.40
LT = 0.35, 0.53 LT = 0.35, 0.53
RT = -0.35, -0.53 RT = -0.35, -0.53
@@ -58,10 +58,6 @@ if __name__ == '__main__':
_inform_controller('stop') _inform_controller('stop')
continue continue
# print trans
# print rot
# continue
movement = [0, 0, 0] movement = [0, 0, 0]
ref_vec = trans[:2] + rot[2:] ref_vec = trans[:2] + rot[2:]
#-1 1 -1 1 -1 1 #-1 1 -1 1 -1 1