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