diff --git a/.gitignore b/.gitignore index 62c8935..238eac7 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ -.idea/ \ No newline at end of file +.idea/ +venv/ \ No newline at end of file diff --git a/code/inverse-kinematics-test/inverse_kinematics.py b/code/inverse-kinematics-test/inverse_kinematics.py index 76625f5..02e2ecd 100644 --- a/code/inverse-kinematics-test/inverse_kinematics.py +++ b/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 diff --git a/documentation/src/SUMMARY.md b/documentation/src/SUMMARY.md index 6c591af..194b692 100644 --- a/documentation/src/SUMMARY.md +++ b/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) diff --git a/documentation/src/ch04 b/documentation/src/ch04 new file mode 100644 index 0000000..911bb1f --- /dev/null +++ b/documentation/src/ch04 @@ -0,0 +1 @@ +# Inverse Kinematics Of The Body diff --git a/documentation/src/ch04-02 b/documentation/src/ch04-02 new file mode 100644 index 0000000..911bb1f --- /dev/null +++ b/documentation/src/ch04-02 @@ -0,0 +1 @@ +# Inverse Kinematics Of The Body diff --git a/documentation/src/ch04-02-inver b/documentation/src/ch04-02-inver new file mode 100644 index 0000000..911bb1f --- /dev/null +++ b/documentation/src/ch04-02-inver @@ -0,0 +1 @@ +# Inverse Kinematics Of The Body diff --git a/documentation/src/ch04-02-inverse-kine b/documentation/src/ch04-02-inverse-kine new file mode 100644 index 0000000..911bb1f --- /dev/null +++ b/documentation/src/ch04-02-inverse-kine @@ -0,0 +1 @@ +# Inverse Kinematics Of The Body diff --git a/documentation/src/ch04-02-inverse-kinematics-body b/documentation/src/ch04-02-inverse-kinematics-body new file mode 100644 index 0000000..911bb1f --- /dev/null +++ b/documentation/src/ch04-02-inverse-kinematics-body @@ -0,0 +1 @@ +# Inverse Kinematics Of The Body diff --git a/documentation/src/ch04-02-inverse-kinematics-body.md b/documentation/src/ch04-02-inverse-kinematics-body.md new file mode 100644 index 0000000..180c724 --- /dev/null +++ b/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}$$ \ No newline at end of file