# Lets talk about the kinematic model.

A project log for DIY hobby servos quadruped robot

Cheap 3D printed 4 legged robot, that almost looks like boston dynamics spot but moves like a newborn.

In this first 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. This model is very looking forward, in real robotics first is to make a robust state stimator so you can have smooth meassurements of robots states.

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).

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:

And the code goes as follow:

```def solve_R(coord , coxa , femur , tibia):
"""where:
coord = np.array([x4,y4,z4])
coxa = L1
femur = L2
tibia = L3
"""
D = (coord**2+(-coord)**2-coxa**2+(-coord)**2-femur**2-tibia**2)/(2*tibia*femur)
D = checkdomain(D) #This is just to deal with NaN solutions
gamma = numpy.arctan2(-numpy.sqrt(1-D**2),D)
tetta = -numpy.arctan2(coord,coord)-numpy.arctan2(numpy.sqrt(coord**2+(-coord)**2-coxa**2),-coxa)
alpha = numpy.arctan2(-coord,numpy.sqrt(coord**2+(-coord)**2-coxa**2))-numpy.arctan2(tibia*numpy.sin(gamma),femur+tibia*numpy.cos(gamma))
angles = numpy.array([-tetta, alpha, gamma])
return angles```

Now, we can locate foot in the 3D space, inside the frame0.

Next step is to move the 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 is the vector from body frame to 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 for 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, shown at the next step:

You can see how this woks on the geometrics.py file, where the fuctions for the rototranslations are defined. You only need to multiply these matrix by the vector you want to transform:

```def RTmatrix(orientation, position):
"""compose translation and rotation"""
roll = orientation
pitch = orientation
yaw = orientation
x0 = position
y0 = position
z0 = position

translation = np.matrix([[1, 0, 0, x0],#Translation matrix
[0, 1, 0, y0],
[0, 0, 1, z0],
[0, 0, 0,  1]])
rotation = Rxyz(roll, pitch, yaw)#rotation matrix

return rotation*translation

def transform(coord,rotation,translation):
"""transforms a vector to a desire rotation and translation"""
vector = np.array([[coord],
[coord],
[coord],
[      1]])

tranformVector = RTmatrix(rotation,translation)*vector
return np.array([tranformVector[0,0], tranformVector[1,0], tranformVector[2,0]])```

Here there is a figure to ilustrate the rotation of the body with respect to the feet in the pitch rotation:

As the the principal frame is transformed, all other child frames are also transformed,  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. This ends with all frames transformed in order to mantein the the frame4 at the same position with respect the BodyFrame.

Here is the code solving that:

```    def solve(self, orn , pos , bodytoFeet):
bodytoFR4 = np.asarray([bodytoFeet[0,0],bodytoFeet[0,1],bodytoFeet[0,2]])
bodytoFL4 = np.asarray([bodytoFeet[1,0],bodytoFeet[1,1],bodytoFeet[1,2]])
bodytoBR4 = np.asarray([bodytoFeet[2,0],bodytoFeet[2,1],bodytoFeet[2,2]])
bodytoBL4 = np.asarray([bodytoFeet[3,0],bodytoFeet[3,1],bodytoFeet[3,2]])

"""defines 4 vertices which rotates with the body"""
_bodytoFR0 = geo.transform(self.bodytoFR0 , orn, pos)
_bodytoFL0 = geo.transform(self.bodytoFL0 , orn, pos)
_bodytoBR0 = geo.transform(self.bodytoBR0 , orn, pos)
_bodytoBL0 = geo.transform(self.bodytoBL0 , orn, pos)
"""defines coxa_frame to foot_frame leg vector neccesary for IK"""
FRcoord = bodytoFR4 - _bodytoFR0
FLcoord = bodytoFL4 - _bodytoFL0
BRcoord = bodytoBR4 - _bodytoBR0
BLcoord = bodytoBL4 - _bodytoBL0
"""undo transformation of leg vector to keep feet still"""
undoOrn = -orn
undoPos = -pos
_FRcoord = geo.transform(FRcoord , undoOrn, undoPos)
_FLcoord = geo.transform(FLcoord , undoOrn, undoPos)
_BRcoord = geo.transform(BRcoord , undoOrn, undoPos)
_BLcoord = geo.transform(BLcoord , undoOrn, undoPos)

"""solve inverse kinematics with frame0 to frame4 vector"""
FR_angles = IK.solve_R(_FRcoord , self.coxa , self.femur , self.tibia)
FL_angles = IK.solve_L(_FLcoord , self.coxa , self.femur , self.tibia)
BR_angles = IK.solve_R(_BRcoord , self.coxa , self.femur , self.tibia)
BL_angles = IK.solve_L(_BLcoord , self.coxa , self.femur , self.tibia)

_bodytofeetFR = _bodytoFR0 + _FRcoord
_bodytofeetFL = _bodytoFL0 + _FLcoord
_bodytofeetBR = _bodytoBR0 + _BRcoord
_bodytofeetBL = _bodytoBL0 + _BLcoord
_bodytofeet = np.matrix([[_bodytofeetFR , _bodytofeetFR , _bodytofeetFR],
[_bodytofeetFL , _bodytofeetFL , _bodytofeetFL],
[_bodytofeetBR , _bodytofeetBR , _bodytofeetBR],
[_bodytofeetBL , _bodytofeetBL , _bodytofeetBL]])

return FR_angles, FL_angles, BR_angles, BL_angles , _bodytofeet```

Here there is a video showing the model in action:

As you see in the video, you can define a initial position for the feet and vary the 6 dof on the body frame, that it may be, for example, the states of a control model.

## Discussions 