From 4d7199bd5710c7be9d84d05f7748dba2e87303f7 Mon Sep 17 00:00:00 2001 From: DeDiamondPro <67508414+DeDiamondPro@users.noreply.github.com> Date: Fri, 15 Mar 2024 12:56:04 +0100 Subject: [PATCH] docs --- documentation/src/SUMMARY.md | 7 ++- .../src/ch04-00-inverse-kinematics.md | 4 ++ .../src/ch04-01-inverse-kinematics-legs.md | 54 ++++++++++++++++--- .../src/ch04-02-inverse-kinematics-body.md | 19 +++++-- documentation/src/title-page.md | 10 +++- 5 files changed, 78 insertions(+), 16 deletions(-) diff --git a/documentation/src/SUMMARY.md b/documentation/src/SUMMARY.md index 194b692..35ff54c 100644 --- a/documentation/src/SUMMARY.md +++ b/documentation/src/SUMMARY.md @@ -1,9 +1,12 @@ # AQLARP [AQLARP](title-page.md) - +# Introduction +- [Design Principles](ch01-01-design-principles.md) +# Building +- [Materials](ch02-01-required-materials.md) # Documentation - [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) + - [Inverse Kinematics Of The Body](ch04-02-inverse-kinematics-body.md) \ No newline at end of file diff --git a/documentation/src/ch04-00-inverse-kinematics.md b/documentation/src/ch04-00-inverse-kinematics.md index e69de29..9d8849b 100644 --- a/documentation/src/ch04-00-inverse-kinematics.md +++ b/documentation/src/ch04-00-inverse-kinematics.md @@ -0,0 +1,4 @@ +# Inverse Kinematics +In this chapter we will look at the calculations behind the inverse kinematics of AQLARP, these calculations are based on trigonometry since it is quite easy to split all these problems into triangles. +If you are not familiar with this topic or want more info, I would recommend watching the video below as it does a great job explaining this concept in a simple manner. + \ No newline at end of file diff --git a/documentation/src/ch04-01-inverse-kinematics-legs.md b/documentation/src/ch04-01-inverse-kinematics-legs.md index 2921fd2..ac69fb1 100644 --- a/documentation/src/ch04-01-inverse-kinematics-legs.md +++ b/documentation/src/ch04-01-inverse-kinematics-legs.md @@ -1,5 +1,5 @@ # Inverse kinematics of the legs -In this chapter we will go over the inverse kinematic calculations for an individual leg +In this chapter we will go over the inverse kinematic calculations for an individual leg. ## Definitions \\(l_{1}\\) Length of the femur (top part of leg) @@ -27,10 +27,48 @@ $$A = \arctan\left( \frac{x}{y} \right)$$ ## Z-position calculation The following calculation calculates the angle of the sideways joint for a given y-position \\(y\\) and a given z-position \\(z\\): $$C = \arctan\left( \frac{z}{y} \right)$$ -## Final calculations -To combine the calculations, we must adjust the y variable in the y-position calculations by dividing it with the cosine of the x- and z-position calculations like this \\(\dfrac{y}{\cos(\arctan(\frac{x}{y}))\cos(\arctan(\frac{z}{y}))}\\). Then since \\(\cos(\arctan(x))\\) simplifies to \\(\dfrac{1}{\sqrt{1+x^2}}\\) we can simplify our calculation to \\(y\sqrt{1+\left(\frac{x}{y}\right)^2}\sqrt{1+\left(\frac{z}{y}\right)^2}\\). This can then further be simplified to \\(\dfrac{\sqrt{(y^2+x^2)(y^2+z^2)}}{y}\\). - -After filling that into the calculations and simplifying a bit further the following calculations are reached: -$$A = \arccos\left( \dfrac{(y^2+x^2)(y^2+z^2) + y^2l_{1}^2 - y^2l_{2}^2}{2yl_{1}\sqrt{(y^2+x^2)(y^2+z^2)}} \right) + \arctan\left( \frac{x}{y} \right)$$ -$$B = \arccos\left( \dfrac{y^2l_{1}^2+y^2l_{2}^2-(y^2+x^2)(y^2+z^2)}{2y^2l_{1}l_{2}} \right)$$ -$$C = \arctan\left( \frac{z}{y} \right)$$ \ No newline at end of file +## Combing Calculations +To combine these calculations we first do the X-position and Z-position calculation. +$$A_{x} = \arctan\left( \frac{x}{y} \right)$$ +$$C_{z} = \arctan\left( \frac{z}{y} \right)$$ +First, since our rotation joint is moved inwards in relation to the leg, we must adjust the height to take the height the rotation joint is creating into account. We can do this as follows: +$$y = y \pm \sin(A_{z}) \times 3$$ +Then we must adjust our y value to take into account that we will have to extend our leg further if we want to remain at the same height but go further forward or to the side. This is done as follows: +$$y = \frac{y}{\cos(A_{x}) \times \cos(C_{z})}$$ +Then we can do our Y-position calculations with this adjusted \\(y\\) value. +$$A_{y} =\arccos\left(\frac{y^2+l_{1}^2-l_{2}^2}{2yl_{1}}\right)$$ +$$B_{y} =\arccos\left(\frac{l_{1}^2+l_{2}^2-y^2}{2l_{1}l_{2}}\right)$$ +Then finally we can combine them by adding them together. +$$A = A_{x} + A_{y}$$ +$$B = B_{y}$$ +$$C = C_{y} + C_{z}$$ +## In code +After transforming all these calculations into code, we get this: +```py +def calc_angles(leg, x, y, z): + # Calculate top joint position to reach X coordinate + x_pos_a = atan(-x / y) + # Calculate side joint position to reach Z coordinate + z_pos_c = atan(z / y) + # Adjust the target height because of the Z-joint having 3cm offset until the leg + if leg == 0 or leg == 3: + y += sin(z_pos_c) * 3 + else: + y -= sin(z_pos_c) * 3 + # Adjust the target height for the X and Z offsets + y = y / (cos(x_pos_a) * cos(z_pos_c)) + # Calculate top joint position to reach Y coordinate + y_pos_a = acos((y * y + leg_length1 * leg_length1 - leg_length2 * leg_length2) / (2 * y * leg_length1)) + # Calculate bottom joint position to reach Y coordinate + y_pos_b = acos((leg_length1 * leg_length1 + leg_length2 * leg_length2 - y * y) / (2 * leg_length1 * leg_length2)) + # Merge all angles and convert them from radians to degrees + A = degrees(x_pos_a + y_pos_a) + B = degrees(y_pos_b) + C = degrees(z_pos_c) + # Adjust the angles for the servo orientation and return the result + return ( + (90 - A) if leg == 0 or leg == 3 else (90 + A), + B if leg == 0 or leg == 3 else (180 - B), + (90 + C) if leg < 2 else (90 - C) + ) +``` \ No newline at end of file diff --git a/documentation/src/ch04-02-inverse-kinematics-body.md b/documentation/src/ch04-02-inverse-kinematics-body.md index 5159e20..7fd522c 100644 --- a/documentation/src/ch04-02-inverse-kinematics-body.md +++ b/documentation/src/ch04-02-inverse-kinematics-body.md @@ -1,4 +1,5 @@ # Inverse kinematic of the body +The inverse kinematics of the body is to transform pitch, roll and rotation into offsets for the x, y and z coordinate of a leg. ## Definitions \\(l\\) length of the body @@ -6,7 +7,7 @@ \\(pitch\\) desired pitch -\\(yaw\\) desired yaw +\\(roll\\) desired roll \\(rotation\\) desired rotation @@ -17,10 +18,20 @@ \\(m_{y}\\) leg movement y \\(m_{z}\\) leg movement z + +\\(x\\) x position of a leg relative to the center + +\\(z\\) z position of a leg relative to the center ## Pitch +The calculations for pitch are as follows: $$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}$$ +## Roll +The calculations for roll are as follows: +$$m_{z} = \frac{w(1-\cos(roll))}{2}$$ +$$m_{y} = \frac{w\sin(roll)}{2}$$ ## Rotation +The calculations for rotation are based on a [rotation matrix](https://en.wikipedia.org/wiki/Rotation_matrix). These calculations are as follows: + +$$m_{x} = (x \times \cos(rotation) - z \times \sin(rotation)) - x$$ +$$m_{z} = (x \times \sin(rotation) + z \times \cos(rotation)) - z$$ \ No newline at end of file diff --git a/documentation/src/title-page.md b/documentation/src/title-page.md index bff5845..7b92f54 100644 --- a/documentation/src/title-page.md +++ b/documentation/src/title-page.md @@ -1,6 +1,12 @@ # AQLARP +Welcome to the AQLARP documentation. This documentation is built with [mdBook](https://github.com/rust-lang/mdBook) + AQLARP stands for **A**ffordable **Q**uadruped **L**earning **A**nd **R**esearch **P**latform. -AQLARP is licensed under the (TODO: insert license here) license. +AQLARP is licensed under the [MIT license](https://github.com/DeDiamondPro/AQLARP/blob/master/LICENSE). + +
+This documentation is incomplete -This documentation is still a work in progress. \ No newline at end of file +It is advisable to have a basic understanding of the parts of the robot to effectively address encountered issues. If you need help, feel free to open an issue on [AQLARP's GitHub](https://github.com/DeDiamondPro/AQLARP). However no guarantees can be provided that I will be able to help you. +
\ No newline at end of file