start of body inverse kinematics

.gitignore

@@ -1 +1,2 @@
-.idea/
+.idea/
+venv/

code/inverse-kinematics-test/inverse_kinematics.py

@@ -1,5 +1,18 @@
 from math import *
 
+pitch = 10.0
+yaw = 10.0
+l = 20.0
+b = 10.0
+
+height_diff_pitch = l * sin(radians(pitch)) / 2
+height_diff_yaw = b * sin(radians(yaw)) / 2
+leg_movement_x = l * (1 - cos(radians(pitch))) / 2
+leg_movement_y = b * (1 - cos(radians(yaw))) / 2
+
+print("Height difference pitch: {}, height difference yaw: {}".format(height_diff_pitch, height_diff_yaw))
+print("Leg movement x: {}, leg movement y: {}".format(leg_movement_x, leg_movement_y))
+
 l1 = 10.0
 l2 = 10.0
 

documentation/src/SUMMARY.md

@@ -6,3 +6,4 @@
 
 - [Inverse Kinematics](ch04-00-inverse-kinematics.md)
 	- [Inverse Kinematics Of The Legs](ch04-01-inverse-kinematics-legs.md)
+	- [Inverse Kinematics Of The Body](ch04-02-inverse-kinematics-body.md)

documentation/src/ch04

@@ -0,0 +1 @@
+# Inverse Kinematics Of The Body

documentation/src/ch04-02

@@ -0,0 +1 @@
+# Inverse Kinematics Of The Body

documentation/src/ch04-02-inver

@@ -0,0 +1 @@
+# Inverse Kinematics Of The Body

documentation/src/ch04-02-inverse-kine

@@ -0,0 +1 @@
+# Inverse Kinematics Of The Body

documentation/src/ch04-02-inverse-kinematics-body

@@ -0,0 +1 @@
+# Inverse Kinematics Of The Body

documentation/src/ch04-02-inverse-kinematics-body.md

@@ -0,0 +1,23 @@
+# Inverse kinematic of the body
+## Definitions
+\\(l\\) length of the body
+
+\\(w\\) width of the body
+
+\\(pitch\\) desired pitch
+
+\\(yaw\\) desired yaw
+
+\\(rotation\\) desired rotation
+
+\\(m_{x}\\) leg movement x
+
+\\(m_{y}\\) leg movement y
+
+\\(m_{z}\\) leg movement z
+## Pitch
+$$m_{x} = \frac{l(1-\cos(pitch))}{2}$$
+$$m_{y} = \frac{l\sin(pitch)}{2}$$
+## Yaw
+$$m_{z} = \frac{w(1-\cos(yaw))}{2}$$
+$$m_{y} = \frac{w\sin(yaw)}{2}$$