In this log i'm going to explain how the kinematic model works.
First thing to take into account is the robot geometry and its DOFs, to ilustrate this, in the next image you see where are they and how they transform.
NOTE: All vector are defined inside the robot frame, no world frame is used for now.
So, we have 4 frames on each leg and one which is commun for all, which is the geometric center of the robot (very close to the CoM).
For our setup, there are 2 important frames on the leg, which are the frame0 (coxa frame or first frame on the leg) and frame4 (foot frame or last frame on the leg).
So now, we are going to calculate Inverse Kinematics between those frames. If you want more information on how to calculate IK you can read this article: https://www.researchgate.net/publication/320307716_Inverse_Kinematic_Analysis_Of_A_Quadruped_Robot
As u can see the setup is a bit diferent, but it only change the simetry of the legs, so the analytic function is the same.
Inverse Kinematics are solved on the IK_solver.py file, which is basically two functions, one for right legs and the other for the left legs. These functions works as shown:
Now, we can locate foot in the 3D space, inside the frame0.
Next step is to move the first dof (body frame) in relation with the feet. Which is essentialy the kinematic model i built, kinematic_model.py file in the code.
For this, we need to define 3 vectors, the first is a 'constant' vector, which is the vector from the center to the frame0 or coxa frame, this vector is going to be rotated and translated with the desired body position.
The next vector is the vector from the center or body frame to the frame4 or foot frame, this vector is where we want the foot to stay.
With this two vector we can calculate the IK vector needed to calculate the inverse kinematics just by subtracting body to frame4 and body to frame0.
Now, we want the body to rotate on its 3 dofs and its 3 logitudinal movements, for this, we just multiply the body to frame0 vector by the rototranslation matrix. You can see how this woks on the geometrics.py file, where the fuctions for the rototranslations are defined.
As the the principal frame is transformed, all other child frames are also transformed (not the body to frame4, as it is virtually defined), so, in order to keep feet still, we need to undo the fransformation for the frame0 to frame4 vector (IK vector) before IK are solved.
Here there is a video showing the model in action: