Implemented yaw rotation while walking

hexapod_python/hexapod_python/taranis_test.py

@@ -55,6 +55,7 @@ class TaranisTestNode(IKNode):
             if(msg.se == 2):
                 self.mode = self.MODE_WALKING
                 self.second_step = False
+                self.last_alpha = 0
                 self.last_delta = 0
                 self.walking_base_pose = copy.deepcopy(self.current_pose_)
                 print("-> MODE_WALKING")
@@ -85,16 +86,25 @@ class TaranisTestNode(IKNode):
                 print("-> MODE_STANDING")
                 return
 
+            alpha = -msg.rudder * math.pi / 12 # +/- 30 degrees max
+
             delta_x = msg.aileron * 100.0 
             delta_y = msg.elevator * 100.0 
 
+            if alpha > 0 and alpha < 0.01:
+                alpha = 0
+            elif alpha < 0 and alpha > -0.01:
+                alpha = 0
+                
             delta = math.sqrt(delta_x**2 + delta_y**2)
 
             if delta < 1:
                 delta_x = 0.0
                 delta_y = 0.0
+                delta = 0
+
 
-            if delta > 10 or self.last_delta > 0:
+            if delta != 0 or alpha != 0 or self.last_delta != 0 or self.last_alpha != 0:
                 step_height = int(50 + 50 * msg.s1)
                 interpolation_steps = int(60 + 50 * msg.s2)
 
@@ -104,19 +114,32 @@ class TaranisTestNode(IKNode):
                 step_pose = copy.deepcopy(self.walking_base_pose)
                 step_fn = {}
 
-                for leg in self.leg_groups_:
+                for leg in self.leg_groups_:                    
+                    x = step_pose['legs'][leg].x
+                    y = step_pose['legs'][leg].y
+
                     if (leg in lift_leg_group) != self.second_step :
-                        step_pose['legs'][leg].x += delta_x / 2
-                        step_pose['legs'][leg].y += delta_y / 2
+                        x += delta_x / 2
+                        y += delta_y / 2
+                        x = x * math.cos(alpha) - y * math.sin(alpha)
+                        y = x * math.sin(alpha) + y + math.cos(alpha)
                         step_fn[leg] = fn
                     else:
-                        step_pose['legs'][leg].x -= delta_x / 2
-                        step_pose['legs'][leg].y -= delta_y / 2
+                        x -= delta_x / 2
+                        y -= delta_y / 2
+                        x = x * math.cos(-alpha) - y * math.sin(-alpha)
+                        y = x * math.sin(-alpha) + y + math.cos(-alpha)
+
+
+                    step_pose['legs'][leg].x = x
+                    step_pose['legs'][leg].y = y             
+
 
                 poses = interpolate_pose(self.current_pose_, step_pose, interpolation_steps, step_fn)
                 self.follow_poses(poses)
                 self.second_step = not self.second_step
                 self.last_delta = delta
+                self.last_alpha = alpha