Monday, July 27, 2015

Creating a Robotics Simulator


Introduction

If you start writing code for a robot it is inevitable to go through a number of iterations until it works as planned. Arduino already makes this easier but especially with complex walking robots it can be a tedious task to reprogram it again and again. This is where a simulator can be very useful.
For my quadruped robot project I programmed a simulator using Processing. I didn't describe it in more detail at that time but I want to make up for that now.

Processing

I mentioned Processing in almost every blog post so far. That's because I think it is a really powerful  yet easy to learn programming language/IDE and it's a shame that it only has such a small community compared to Arduino. But what makes it special?
  • The IDE is only ~200mb and can be run from a USB drive. Compared to Matlab or Visual Studio this is definitely an advantage.
  • The IDE is very similar to Arduino and so is the language, As long as you are not using arrays or hardware libraries you can copy and past blocks of code between Arduino and Processing. 
  • Code can be compiled for Windows, Linux, Mac and even Android with very few alterations. (http://coretechrobotics.blogspot.com/2014/08/a-universal-bluetooth-remote-app.html)
  • The community may be small but you still can find tons of examples and tutorials online. Also there are libraries for anything, just like with Arduino.

Programming the Simulator

1. Basics

First you need to download and unpack Processing:
https://processing.org/download/?processing    (This tutorial is based on Processing 2.2.1)
Be sure that the newest Java version is installed as well.
When this is done start the Processing IDE.

We will start with drawing a simple rectangle.
There are two basic functions that need to be in a sketch. Setup() is called once on startup and draw() runs until you close the sketch window. In the setup function we need to define the size of the sketch in pixels. For this tutorial we will stick with 1200x800. Now we already have a working sketch. Next we insert a rectangle by using rect(). The four parameters for this function are x-position, y-position, x-size and y-size. There are other shapes like ellipses that follow the same scheme. This diagram from the processing website describes the directions in 2D and 3D space:

Additionally you can fill the rectangle with a color by using fill(). Under Tools/Color Selector you can pick a color and paste its value as a parameter. It is important to use fill() before drawing the rectangle. Changes in something like color or stroke only affect the subsequent shapes.
We won't need outlines for this, so we will eliminate them with noStroke().

void setup(){
    size(1200, 800);
}

void draw(){  
   fill(#FF9F03);
   noStroke();
   rect(100, 100, 500, 500);
}



2. Make it 3D

Turning the square into a box requires a few changes. We need to change to a 3D renderer by adding OPENGL to the size function. Box(size) creates an equal sided cube at the origin (top left corner). The function translate(x, y, z) can be used to move it away from the corner. Rotation is done using rotateX(angle), rotateY(angle) and rotateZ(angle). width and height are referencing the values we added to the size function, translate(width/2, height/2) always makes the cube appear at the center.
To enable anti aliasing we need to call the smooth() function. This will not work without adding background(color), that gets called every cycle to overwrite the screen. Lights() turns the lights on and add shades to the cube.

void setup(){
    size(1200, 800, OPENGL);
}

void draw(){  
   background(32);
   smooth();
   lights();
   
   fill(#FF9F03);
   noStroke();
   
   translate(width/2, height/2);
   rotateX(-0.5);
   rotateY(0.5);
   box(300);
}



3. Mouse controls

3D is kind of boring if you can't interact with it. The easiest way to do this is replacing the fixed rotation values with the mouse position to rotate the cube around while the sketch is running. We need to create two variables, rotX and rotY that well be used as view rotation. The function mouseDragged() is used to write the mouse position to these variables while a mouse button is pressed.

float rotX, rotY;

void setup(){
    size(1200, 800, OPENGL);
}

void draw(){  
   background(32);
   smooth();
   lights();
   
   fill(#FF9F03);
   noStroke();
   
   translate(width/2, height/2);
   rotateX(rotX);
   rotateY(-rotY); 
   box(300);
}

void mouseDragged(){
    rotY -= (mouseX - pmouseX) * 0.01;
    rotX -= (mouseY - pmouseY) * 0.01;
}



4. Importing geometry

Unless you are building Wall-E, a cube won't be a good representation of your robot. Luckily Processing is able to import various 3D files including .obj-files.
For the next steps you will have to download the parts I prepared:
https://www.dropbox.com/s/ymn59u6qw7zbjyi/robot%20parts.zip?dl=1
Create a new folder in the direction of your sketch file and name it "data". Unpack the 5 obj-files to that folder.
We can now import these objects to our sketch by creating a PShape for each of them and using loadShape("file") to assign the obj-file. Replace the box with shape(base) and Processing will draw the geometry. Depending on the units we will have to scale(factor) the object to better fit the screen. I also used the translate command to position the part lower on the screen because otherwise the robot would be off center later.

PShape base, shoulder, upArm, loArm, end;
float rotX, rotY;

void setup(){
    size(1200, 800, OPENGL);
    
    base = loadShape("r5.obj");
    shoulder = loadShape("r1.obj");
    upArm = loadShape("r2.obj");
    loArm = loadShape("r3.obj");
    end = loadShape("r4.obj");
}

void draw(){  
   background(32);
   smooth();
   lights();
   
   noStroke();
   
   translate(width/2,height/2);
   scale(-4);
   translate(0,-40,0);
   rotateX(rotX);
   rotateY(-rotY);    
     shape(base);
}

void mouseDragged(){
    rotY -= (mouseX - pmouseX) * 0.01;
    rotX -= (mouseY - pmouseY) * 0.01;
}



5. Rotating/Aligning multiple parts

Now we will assembly the robot by adding the remaining parts. Use the translate/rotate functions to position the parts. Translation and rotation values will always add up. That means that all parts are in a chain where each link is moved relatively to its predecessor.
If you are using your own robot parts you can find the right translation values in the cad file. If the base is 60mm high you have to translate the next part 60 units and so on. Rotation values are in radians and sometimes it will take a few attempts to find the right ones.
By defining three rotation values as variables we will be able to move the joints in the next step.
If you export your obj-files from a CAD software there will be a second mtl-file containing the color settings and Processing will render it that way. If not, disableStyle() can be used to render objects with the standard fill/stroke setting.

PShape base, shoulder, upArm, loArm, end;
float rotX, rotY;
float alpha = -1, beta = -2, gamma;

void setup(){
    size(1200, 800, OPENGL);
    
    base = loadShape("r5.obj");
    shoulder = loadShape("r1.obj");
    upArm = loadShape("r2.obj");
    loArm = loadShape("r3.obj");
    end = loadShape("r4.obj");
    
    shoulder.disableStyle();
    upArm.disableStyle();
    loArm.disableStyle(); 
}

void draw(){  
   background(32);
   smooth();
   lights();
   
   fill(#FFE308); 
   noStroke();
   
   translate(width/2,height/2);
   scale(-4);
   translate(0,-40,0);
   rotateX(rotX);
   rotateY(-rotY);    
     shape(base);
     
   translate(0, 4, 0);
   rotateY(gamma);
     shape(shoulder);
      
   translate(0, 25, 0);
   rotateY(PI);
   rotateX(alpha);
     shape(upArm);
      
   translate(0, 0, 50);
   rotateY(PI);
   rotateX(beta);
     shape(loArm);
      
   translate(0, 0, -50);
   rotateY(PI);
     shape(end);
}

void mouseDragged(){
    rotY -= (mouseX - pmouseX) * 0.01;
    rotX -= (mouseY - pmouseY) * 0.01;
}



6. Kinematics

For this step we will add a second tab to the sketch where the inverse kinematics and movements are calculated. For this tutorial I reused some of the code from my quadruped robot. Basically, the IK() function converts three coordinates to three angles. SetTime() generates a time value from 0 to 4. WritePos() calls both functions and generates a sine function that looks like a horizontal eight, making for smooth movements of the robot. 
The only thing we need to change in the main sketch tab is calling the writePos() function.
If you look at the code in the second tab, it could easily be run on an arduino without alterations. This is what I did with my quadruped simulator. I tested the code and later copied the entire thing to my Arduino sketch.

Main Tab
PShape base, shoulder, upArm, loArm, end;
float rotX, rotY;
float posX=1, posY=50, posZ=50;
float alpha, beta, gamma;

void setup(){
    size(1200, 800, OPENGL);
    
    base = loadShape("r5.obj");
    shoulder = loadShape("r1.obj");
    upArm = loadShape("r2.obj");
    loArm = loadShape("r3.obj");
    end = loadShape("r4.obj");
    
    shoulder.disableStyle();
    upArm.disableStyle();
    loArm.disableStyle(); 
}

void draw(){ 
   writePos();
   background(32);
   smooth();
   lights();
   
   fill(#FFE308); 
   noStroke();
   
   translate(width/2,height/2);
   rotateX(rotX);
   rotateY(-rotY); 
   scale(-4);
   
   translate(0,-40,0);   
     shape(base);
     
   translate(0, 4, 0);
   rotateY(gamma);
     shape(shoulder);
      
   translate(0, 25, 0);
   rotateY(PI);
   rotateX(alpha);
     shape(upArm);
      
   translate(0, 0, 50);
   rotateY(PI);
   rotateX(beta);
     shape(loArm);
      
   translate(0, 0, -50);
   rotateY(PI);
     shape(end);
}

void mouseDragged(){
    rotY -= (mouseX - pmouseX) * 0.01;
    rotX -= (mouseY - pmouseY) * 0.01;
}


Inverse Kinematics Tab
float F = 50;
float T = 70;
float millisOld, gTime, gSpeed = 4;

void IK(){

  float X = posX;
  float Y = posY;
  float Z = posZ;

  float L = sqrt(Y*Y+X*X);
  float dia = sqrt(Z*Z+L*L);

  alpha = PI/2-(atan2(L, Z)+acos((T*T-F*F-dia*dia)/(-2*F*dia)));
  beta = -PI+acos((dia*dia-T*T-F*F)/(-2*F*T));
  gamma = atan2(Y, X);

}

void setTime(){
  gTime += ((float)millis()/1000 - millisOld)*(gSpeed/4);
  if(gTime >= 4)  gTime = 0;  
  millisOld = (float)millis()/1000;
}

void writePos(){
  IK();
  setTime();
  posX = sin(gTime*PI/2)*20;
  posZ = sin(gTime*PI)*10;
}



7. Final touches

With the code above this is already a fully functional robotics simulator. But there are tons of other things to do with processing. For the last step I added an effect that was supposed to look like a spray can. It ended up a little differently but still looks nice. I also added a directional light, which makes the robot appear a little more realistic.
If you want you can export the entire project to a executable program for windows or any other operating system by clicking on "Export Application".

PShape base, shoulder, upArm, loArm, end;
float rotX, rotY;
float posX=1, posY=50, posZ=50;
float alpha, beta, gamma;

float[] Xsphere = new float[99];
float[] Ysphere = new float[99];
float[] Zsphere = new float[99];

void setup(){
    size(1200, 800, OPENGL);
    
    base = loadShape("r5.obj");
    shoulder = loadShape("r1.obj");
    upArm = loadShape("r2.obj");
    loArm = loadShape("r3.obj");
    end = loadShape("r4.obj");
    
    shoulder.disableStyle();
    upArm.disableStyle();
    loArm.disableStyle(); 
}

void draw(){ 
   writePos();
   background(32);
   smooth();
   lights(); 
   directionalLight(51, 102, 126, -1, 0, 0);
    
    for (int i=0; i< Xsphere.length - 1; i++) {
    Xsphere[i] = Xsphere[i + 1];
    Ysphere[i] = Ysphere[i + 1];
    Zsphere[i] = Zsphere[i + 1];
    }
    
    Xsphere[Xsphere.length - 1] = posX;
    Ysphere[Ysphere.length - 1] = posY;
    Zsphere[Zsphere.length - 1] = posZ;
   
   
   noStroke();
   
   translate(width/2,height/2);
   rotateX(rotX);
   rotateY(-rotY);
   scale(-4);
   
   for (int i=0; i < Xsphere.length; i++) {
     pushMatrix();
     translate(-Ysphere[i], -Zsphere[i]-11, -Xsphere[i]);
     fill (#D003FF, 25);
     sphere (float(i) / 20);
     popMatrix();
    }
    
   fill(#FFE308);  
   translate(0,-40,0);   
     shape(base);
     
   translate(0, 4, 0);
   rotateY(gamma);
     shape(shoulder);
      
   translate(0, 25, 0);
   rotateY(PI);
   rotateX(alpha);
     shape(upArm);
      
   translate(0, 0, 50);
   rotateY(PI);
   rotateX(beta);
     shape(loArm);
      
   translate(0, 0, -50);
   rotateY(PI);
     shape(end);
}

void mouseDragged(){
    rotY -= (mouseX - pmouseX) * 0.01;
    rotX -= (mouseY - pmouseY) * 0.01;
}




You can download the finished simulator from here:
https://www.dropbox.com/s/sr4gk1y5mlxrrid/demoRobot.zip?dl=1

Conclusion

As you can see, programming a simulator like that is no rocket science. If you need help making your own check out the Processing reference https://processing.org/reference/ and the forums http://forum.processing.org/two/. I hope this tutorial was helpful to you and it would be awesome to see more robots in Processing in the future.

11 comments:

  1. Excellent manual. I also try to write my simulator.

    ReplyDelete
    Replies
    1. i see you said this back in August 1, 2015 at 1:45 AM today the year is just about done, what are your results?

      Delete
  2. " I didn't describe it in more detail at that time but I want to make up for that now." you still cop'out all this was done cuz of the one you made for your dog robot so why make this if your not going to show us how the control when for the other application ? this just does its own thing so that no sim for actual application this is just a demo for this particular application, people make sims for control like you said here i have no control except for the cam view angle which is useful but not really needed since its doing the same thing so need to view any other side, in this block of code i try to make the mouse be the controller and all i got was erratic movements clearly identifying that i was not thought anything by reading this entire page.

    translate(0, 0, 50);
    rotateY(mouseY);
    rotateX(beta);
    shape(loArm);

    nothing smooth nothing as i was expecting, but your right this is no rocket science and that's what we want so bring out the next tutorial or blog about how to go about building something like the one found here
    https://www.youtube.com/watch?v=y5ThybI_usE

    ReplyDelete
    Replies
    1. Of course this simulator has no practical purpose.
      It is meant to show you how to get cad-models into processing and manipulate them.
      I didn't cover the part about sliders and buttons (you can even use your keyboard) because that would make the tutorial more complicated than it needs to be.
      Just have a look at http://www.sojamo.de/libraries/controlP5/

      Delete
    2. this here
      void setup(){
      size(1200, 800, OPENGL);

      base = loadShape("r5.obj");
      shoulder = loadShape("r1.obj");
      upArm = loadShape("r2.obj");
      loArm = loadShape("r3.obj");
      end = loadShape("r4.obj");
      }
      is all you had to do if thats all the tutorial was trying to teach, loadShape as well as OBJloader does this same thing this would have been half of page if that s all you was going to talk about but it when in more in depth for no reason because there nothing you can do with this, i would have rather you kill a whole page talking about what you did not and have a great result at the end then do all this for nothing you feel me?

      and when you say "manipulate " this is the one thing you did not do, there is no manipulation in this except for the camera angle moving a arm with the mouse or slider or keyboard is alot more practical by clicking on the desire arm and then having control of its translation on the xy and z, if this would have been done then we can use that to adapt to our own program making your tutorial very valuable but is almost as you show us something that was easy to do to begin with but not show us whats beyond any tutorial out there, its a cool demo but thats about it, cuz you see wheb your on youtube watching your other sim and ou tell us to head over here to show us how that sim was made and then you giving us this i think most ppl are like WTF? lol i know i was

      Delete
  3. there's alot worth going into in Inverse Kinematics Tab alone, at the end of the day if we cannot build a sim we can at lease use our own physical platform to test Inverse Kinematics but if we dont know whats being said we cant really move on with anything, now i actually understand basic trig but not so familiar with square root and am barely learning about Radiance,so it be nice if you do a little breaking down, i just try to bring my own models into the pde and im working on a humanoid so i brought in a single arm "shoulder,bicep,elbow and forearm" everything is everywhere despite the fact that this model alone in its 3D space where it was built "fusion360" its accurate so im not sure why it all becomes a mess when i import

    ReplyDelete
  4. This comment has been removed by the author.

    ReplyDelete
  5. I found this tutorial very usefull!
    I created my own, based off your tutorial:
    (https://github.com/Paul-Ver/RoboticsSimulation)

    Am I allowed to use your 3D models (and code from this tutorial?
    I did change some in the code:
    - Added textfields
    - robot class, which handles all drawing/loading of the robot model.
    - 2D User-interface/buttons
    - a claw :)
    And I'm going to add:
    - serial communication (with an arduino robot)
    - collisions / picking up objects

    But I broke the kinematics while changing the axis (to something logical I guess)

    ReplyDelete
    Replies
    1. Wow, that looks great already!
      The robot class makes the code much cleaner, especially when working with larger assemblies.

      Of course you can use my code and files, as long as you mention my blog.
      I am curious how you will manage to include collisions and physics. I gave up on that topic after I couldn't find a decent library. It would be awesome to be able to simulate walking robots with working physics.
      Keep up the good work!

      Delete
    2. This comment has been removed by the author.

      Delete