-
Project bibliography and interesting papers.
10/02/2020 at 13:42 • 0 commentsHere i'm going to show all the bibliography related with the project and some other reading that has inspired me.
Software documentation:
More mathematical Articles:
-
Small community.
09/28/2020 at 15:32 • 3 commentsThis log is just to show the repercussion of the project and say thanks for the support from all of you that are reading this project.
This is awesome to see that the hard work you made can be very useful for others. Even to the point of creating a new platform with a small community that is growing every day.
This project has let me see the values of the OPEN SOURCE community, how easy is nowadays to develop a new platform, each contributor from very different places in the word but with one commun interest that is learn things in order to make our lives more interesting.
Here are some of Makers that supported the project since the first day and they already have a small Chop at their homes:
Thanks to @OilSpigot
(min 4:00 to see his robot in action)
Thanks to @Creasento:
-
Bill of materials and price of the robot.
09/19/2020 at 12:33 • 0 commentsTo complete the Component Section, in this log i want to show the complete bill of materials, with the price for one unit of the robot, i have to say i'm surprised how cheap it is to build just one robot.
In my case, this development has been more expensive as i have made 3 different prototypes with its resvective components, so more or less i have spent two times the price of one unit.
Almost all componet were bought from the typical chenese online wareshops.
Once you have all the components, the building procress is not very hard if you have experience with 3D printed prototyping.
Go to Instruction Section
-
PyBullet Simulation example
09/15/2020 at 13:06 • 0 commentsThis log i'm going to talk about the simulation used for the robot development, this was mostly used for debugging the maths inside the the different models.
The code of the simulation is no longer implemented in the Raspberry code, as it can't support GUI mode and i'm not taking data from PyBullet environment. The code i am referring to is at the V1_PCsimulation branch of the robot GitHub repository.
This code can be ran with the robot just plugging the arduino to the PC via USB and run the robot_walking.py file, all requirements at README.mdBut if you don't have the robot built, you can run just the simulation a play along with the robot inside the PyBullet environment. Also feel free to modify it and improve it.
You just need PyBullet and Numpy on Python 3, then download the V1_PCsimulation branch and run walking_simulation_example.py.
More info of how the simulation works shown in the next video:
Now, i'm going to breightly explain the main code of the simulation, here is shown:
""" Created on Sun Mar 15 19:51:40 2020 @author: linux-asd """ import pybullet as p import numpy as np import time import pybullet_data from pybullet_debuger import pybulletDebug from kinematic_model import robotKinematics from gaitPlanner import trotGait """Connect to rhe physics client, you can choose GUI or DIRECT mode""" physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally """set Gravity""" p.setGravity(0,0,-9.8) """import the robot's URDF file, if it is fixed no ground plane is needed""" cubeStartPos = [0,0,0.2] FixedBase = False #if fixed no plane is imported if (FixedBase == False): p.loadURDF("plane.urdf") boxId = p.loadURDF("4leggedRobot.urdf",cubeStartPos, useFixedBase=FixedBase) jointIds = [] paramIds = [] time.sleep(0.5) """get info along all the joints for debugging if needed""" for j in range(p.getNumJoints(boxId)): # p.changeDynamics(boxId, j, linearDamping=0, angularDamping=0) info = p.getJointInfo(boxId, j) print(info) jointName = info[1] jointType = info[2] jointIds.append(j) footFR_index = 3 footFL_index = 7 footBR_index = 11 footBL_index = 15 """init the classes used""" pybulletDebug = pybulletDebug() robotKinematics = robotKinematics() trot = trotGait() #robot properties """initial foot position""" #foot separation (Ydist = 0.16 -> tetta=0) and distance to floor Xdist = 0.20 Ydist = 0.15 height = 0.15 """This matrix will define where the feet are going to be at its standing position with respect the center of the robot""" #body frame to foot frame vector bodytoFeet0 = np.matrix([[ Xdist/2 , -Ydist/2 , -height], [ Xdist/2 , Ydist/2 , -height], [-Xdist/2 , -Ydist/2 , -height], [-Xdist/2 , Ydist/2 , -height]]) """Gait definition parameters, here you can set different period of step and the offset between every foot's loop, defining different gaits""" T = 0.5 #period of time (in seconds) of every step offset = np.array([0.5 , 0. , 0. , 0.5]) #defines the offset between each foot step in this order (FR,FL,BR,BL) """start a real time simulation, so no simulation steps are needed""" p.setRealTimeSimulation(1) while(True): lastTime = time.time() """show sliders in the GUI""" pos , orn , L , angle , Lrot , T = pybulletDebug.cam_and_robotstates(boxId) #calculates the feet coord for gait, defining length of the step and direction (0º -> forward; 180º -> backward) bodytoFeet = trot.loop(L , angle , Lrot , T , offset , bodytoFeet0) ##################################################################################### ##### kinematics Model: Input body orientation, deviation and foot position #### ##### and get the angles, neccesary to reach that position, for every joint #### FR_angles, FL_angles, BR_angles, BL_angles , transformedBodytoFeet = robotKinematics.solve(orn , pos , bodytoFeet) """move all joints at the next time step angle""" #move movable joints for i in range(0, footFR_index): p.setJointMotorControl2(boxId, i, p.POSITION_CONTROL, FR_angles[i - footFR_index]) for i in range(footFR_index + 1, footFL_index): p.setJointMotorControl2(boxId, i, p.POSITION_CONTROL, FL_angles[i - footFL_index]) for i in range(footFL_index + 1, footBR_index): p.setJointMotorControl2(boxId, i, p.POSITION_CONTROL, BR_angles[i - footBR_index]) for i in range(footBR_index + 1, footBL_index): p.setJointMotorControl2(boxId, i, p.POSITION_CONTROL, BL_angles[i - footBL_index]) # print(time.time() - lastTime) p.disconnect()
-
Future 3DoF foot sensor.
08/27/2020 at 20:52 • 0 commentsIn order to make the robot dynamic in the precisse meaning of the word, we need to have meassurements of the forces acting acting inside the robot, this can be either from the actuators, feet or the IMU.
This way we can build a robust control algorithm taking into account these forces, such as the Inverse Pendulum model. Basically what we want in here is to implement a physics math model in stead of a direct model like the Kinematics model it implements now.
This task is not easy to achive, but i found a paper with a very inteligent solution to this problem: Foot design for a hexapod walking robot.
Basically it uses 3 sensitive resistance force sensor, with a proper orientation in order to read the forces at the foot in the 3 different directions.
What i came with is shown in this pictures, but this is only a quick design with lot of errors at the time of reading accurate meassurements.
Some actual works on the MIT cheetah show how usefull can be this type of foot sensor: -
Calibrating the servos.
08/01/2020 at 09:15 • 0 commentsFirst of all, you will need to plug all servos to their proper pin, in my case here they are (you can change this for your how setup):
void connectServos() { //FR Servos[0].attach(40); //coxa Servos[1].attach(38); //femur Servos[2].attach(36); //tibia //FL Servos[3].attach(42); //coxa Servos[4].attach(44); //femur Servos[5].attach(46); //tibia //BR Servos[6].attach(34); //coxa Servos[7].attach(32); //femur Servos[8].attach(30); //tibia //BL Servos[9].attach(48); //coxa Servos[10].attach(50); //femur Servos[11].attach(52); //tibia }
As soon as the servos are corretly attached, what i made in order to put the servos on its correct position is just to dissasembly the legs, i mean, separate the servo output from the its horn, in order to let the servo move freely. This way you can power the servos, let them go to the zero position and the reassembly.
I wrote a small sketch in order to put the servos on their zero position, you can find it at servo_calibration.py file, this way you can attatch the servo very close to their zero position.
Then, a more precise calibration is needed, you will need to calibrate the offset of each servo, those offset are defined at angleToPulse.py file. This file only contains the line defining the pulse corresponding to each angle for every servo.
Being a line in the form: y = Ax + B. The offset of each servo is defined by B, which mean the pulse at the zero angle.
If you are using different servos, the slope of the line (A) will be different so you will need to identify it. What i did, is just to take 4-5 meassurement of pulse at different angles and put them on an excel where i calculate the adjucted line from those points.
def convert(FR_angles, FL_angles, BR_angles, BL_angles): pulse = np.empty([12]) #FR pulse[0] = int(-10.822 * np.rad2deg(-FR_angles[0])) + 950 pulse[1] = int(-10.822 * np.rad2deg(FR_angles[1])) + 2280 pulse[2] = int(10.822 * (np.rad2deg(FR_angles[2]) + 90)) + 1000 #FL pulse[3] = int(10.822 * np.rad2deg(FL_angles[0])) + 1020 pulse[4] = int(10.822 * np.rad2deg(FL_angles[1])) + 570 pulse[5] = int(-10.822 * (np.rad2deg(FL_angles[2]) + 90)) + 1150 #BR pulse[6] = int(10.822 * np.rad2deg(-BR_angles[0])) + 1060 pulse[7] = int(-10.822 * np.rad2deg(BR_angles[1])) + 2335 pulse[8] = int(10.822 * (np.rad2deg(BR_angles[2]) + 90)) + 1200 #BL pulse[9] = int(-10.822 * np.rad2deg(BL_angles[0])) + 890 pulse[10] = int(10.822 * np.rad2deg(BL_angles[1])) + 710 pulse[11] = int(-10.822 * (np.rad2deg(BL_angles[2]) + 90)) + 1050 return pulse
-
A more stable walking.
07/20/2020 at 11:49 • 0 commentsThis log is about polishing the gaitPlanner as you can see in the next video:
What is interesting on this video is that no control algorithm is running, but it seems very stable.
This is achived just by separating stance phase from the swing (as explained in the gaitPlanner.py log), this way i can define an offset between both phases, so running the stance phase at 3/4 of the hole step, makes the robot to hold its weigth on the 4 legs for a short period of time being a bit more stable, just by running the gaitPlanner loop.
Visually the gait is defined like follows:Aditional info about that is that servos got very hot if it runs for more than 10 minutes.
-
Quadruped robot meets my cat.
07/07/2020 at 18:29 • 0 commentsAs a quick update of the V2 robot performing i have introduced it to my cat.
My cat is very introverted and usually doesn't like new things, in this case isn't different but it seems to cause she some curiosity.
The robot performs well, as it uses the last version of the code, the extra weigth of the batteries and raspberry pi is not a problem as the new design is much more lighter and the servos are now powered at 6V in stead of 5V as in the first version.
On the other hand, i have notice the model randomly gives bad solutions running on the raspberry pi, thats why sometimes you see the leg make a strange movement. I haven't identify the problem yet, maybe is my software problem or bufer overflow.
There is no aim to hurt the animal, is my cat omg.
-
Real robot vs PyBullet simulation.
06/26/2020 at 14:45 • 0 commentsIn this video i compare the real robot model with the simulation model:
I'm using PyBullet physics engine, it is simple to use and seems to have a good performance as well as very precise results. In the video you can see that the simulation is a bit slow, this is becouse the simulation (at the time of the video) was not working at real time but computing the simulation every 20 ms.
-
Raspberry Pi communication interfaces.
06/21/2020 at 21:41 • 0 commentsThis log is going to be about the python communication files, explaining the interfaces used in order to communicate both with the ps3 controller and with the arduino.
Serial Bidirectional Communication on python.
This is written in the serial_com.py file and the code goes as follow:
- First thing is to define the serial port and its baudrate.
import serial class ArduinoSerial: def __init__(self , port): #ls -l /dev | grep ACM to identify serial port of the arduino self.arduino = serial.Serial(port, 500000, timeout = 1) self.arduino.setDTR(False) #force an arduino reset time.sleep(1) self.arduino.flushInput() self.arduino.setDTR(True)
- Then the communication is done with the next two fuctions. This fuctions are very general and can be used without big chnges.
This program will stop the main loop until the next data arrives, this way the arudino memory is not overloaded with the pulse commands arriving.
def serialSend(self, pulse): comando = "<{0}#{1}#{2}#{3}#{4}#{5}#{6}#{7}#{8}#{9}#{10}#{11}>" #Input command=comando.format(int(pulse[0]), int(pulse[1]), int(pulse[2]), int(pulse[3]), int(pulse[4]), int(pulse[5]), int(pulse[6]), int(pulse[7]), int(pulse[8]), int(pulse[9]), int(pulse[10]), int(pulse[11])) self.arduino.write(bytes(command , encoding='utf8')) def serialRecive(self): try: startMarker = 60 endMarker = 62 getSerialValue = bytes() x = "z" # any value that is not an end- or startMarker byteCount = -1 # to allow for the fact that the last increment will be one too many # wait for the start character while ord(x) != startMarker: x = self.arduino.read() # save data until the end marker is found while ord(x) != endMarker: if ord(x) != startMarker: getSerialValue = getSerialValue + x byteCount += 1 x = self.arduino.read() loopTime , Xacc , Yacc , roll , pitch = numpy.fromstring(getSerialValue.decode('ascii', errors='replace'), sep = '#' )
Connecting PS3 controller to the Raspberry Pi and reading its events.- Connecting the PS3 controller was very simple, here is the tutorial i used: https://pimylifeup.com/raspberry-pi-playstation-controllers/
- Then events are read on the joystick.py file, using evdev python library. In this set up, the events are read every time the read() function is called, ignoring the others ongoing events. Also there isn't any queue method or filtering.
from evdev import InputDevice, categorize, ecodes from select import select import numpy as np class Joystick: def __init__(self , event): #python3 /usr/local/lib/python3.8/dist-packages/evdev/evtest.py for identify event self.gamepad = InputDevice(event) def read(self): r,w,x = select([self.gamepad.fd], [], [], 0.) if r: for event in self.gamepad.read(): if event.type == ecodes.EV_KEY: if event.value == 1: if event.code == 544:#up arrow self.T += 0.05 if event.code == 545:#down arrow self.T -= 0.05 if event.code == 308:#square if self.compliantMode == True: self.compliantMode = False elif self.compliantMode == False: self.compliantMode = True else: print("boton soltado") elif event.type == ecodes.EV_ABS: absevent = categorize(event) if ecodes.bytype[absevent.event.type][absevent.event.code] == "ABS_X": self.L3[0]=absevent.event.value-127 elif ecodes.bytype[absevent.event.type][absevent.event.code] == "ABS_Y": self.L3[1]=absevent.event.value-127 elif ecodes.bytype[absevent.event.type][absevent.event.code] == "ABS_RX": self.R3[0]=absevent.event.value-127 elif ecodes.bytype[absevent.event.type][absevent.event.code] == "ABS_RY": self.R3[1]=absevent.event.value-127