diff --git a/launch/fulltest.launch b/launch/fulltest.launch index 7e1d693..67d40df 100644 --- a/launch/fulltest.launch +++ b/launch/fulltest.launch @@ -4,19 +4,17 @@ - + + + - - - + --> diff --git a/script/imitator.py b/script/imitator.py index 51ceb9d..adf61c2 100755 --- 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: diff --git a/script/walker.py b/script/walker.py index 063e2a7..5582cdf 100755 --- a/script/walker.py +++ b/script/walker.py @@ -10,7 +10,7 @@ from controller import inform_controller_factory #min #max -FW = -1.65, -1.45 +FW = -1.70, -1.40 BK = -2.20, -2.40 LT = 0.35, 0.53 RT = -0.35, -0.53 @@ -58,10 +58,6 @@ if __name__ == '__main__': _inform_controller('stop') continue - # print trans - # print rot - # continue - movement = [0, 0, 0] ref_vec = trans[:2] + rot[2:] #-1 1 -1 1 -1 1