implemented torso imitation (DANGEROUS!!!)

also fixed usb_cam launch bug
2019-01-30 11:33:13 +01:00
parent b95417293b
commit f050c3464a
3 changed files with 22 additions and 13 deletions
--- a/script/imitator.py
+++ b/script/imitator.py
@@ -30,6 +30,21 @@ if __name__ == '__main__':
        if not _inform_controller('imitate'):
            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):
            try: