ControlOnLine.net

Simulació mes senzilla encara, amb 2D i només dos graus de llibertat. Com l'escara que volem fer, l'altre te mes graus de llibertat i el calcul es per DH.

https://gist.github.com/dustynrobotshttp://www.dustynrobots.com/academia/teaching/robotic-arm-in-processing/

Es molt senzill i ens pot servir de base per al nostre robot:

vis_2d.pde

/*
Planar Robotic Arm Visualizer
 
by Dustyn Roberts 20120622
modified by David Cummings 20120830
 */
import processing.serial.*;
//constants 
float a1 = 198; // shoulder-to-elbow "bone" length from Solidworks (mm)
float a2 = 220; // elbow-to-wrist "bone" length from Solidworks (mm) - longer c bracket
 
boolean elbowup = false; // true=elbow up, false=elbow down
 
int Xoffset = 425;
int Yoffset = 425;
 
//variables
float theta1 = 0.0; // target angles as determined through inverse kinematics
float theta2 = 0.0;
 
float c2 = 0.0; // is btwn -1 and 1
float s2 = 0.0;
 
float joint1X;
float joint1Y;
 
float joint2X;
float joint2Y;
//Serial //myPort;
void setup() {
 
  size(850, 650, P3D);
  background(157, 6, 50);
  ////myPort = new Serial(this, "COM28", 9600);
}
 
void draw() {
  background(123, 82, 171);
  strokeWeight(4);
 
  rotateX(PI); // make the y axis point up 
  translate(Xoffset, -Yoffset); // lower down the arm and move it to the middle so we can see it
 
  // figure out the joint angles needed to get to mouseX, mouseY position
  get_angles(mouseX-Xoffset, -mouseY+Yoffset);  
  //figure out the joint coordinates of joint1 to draw link 1
  get_xy();
  line(0.0, 0.0, joint1X, joint1Y);
  line(joint1X, joint1Y, joint2X, joint2Y);
 
  // print out the x and y values of the end effector
 
  print(mouseX-Xoffset);
  print(',');
  println(-mouseY+Yoffset);
}
 
// Given target(Px, Py) solve for theta1, theta2 (inverse kinematics)
void get_angles(float Px, float Py) {
  // first find theta2 where c2 = cos(theta2) and s2 = sin(theta2)
  c2 = (pow(Px, 2) + pow(Py, 2) - pow(a1, 2) - pow(a2, 2))/(2*a1*a2); // is btwn -1 and 1
 
  if (elbowup == false) {
    s2 = sqrt(1 - pow(c2, 2)); // sqrt can be + or -, and each corresponds to a different orientation
  }
  else if (elbowup == true) {
    s2 = -sqrt(1 - pow(c2, 2));
  }
  theta2 = degrees(atan2(s2, c2)); // solves for the angle in degrees and places in correct quadrant
 
  // now find theta1 where c1 = cos(theta1) and s1 = sin(theta1)
  theta1 = degrees(atan2(-a2*s2*Px + (a1 + a2*c2)*Py, (a1 + a2*c2)*Px + a2*s2*Py));
  print((int)theta1);
  print(',');
  println((int)theta2);
  //myPort.write('e');
  //myPort.write(180-(int)theta2);
  delay(10);
  //myPort.write('s');
  //myPort.write(180-(int)theta1);
}
 
void get_xy() {
  joint1X = a1*cos(radians(theta1));
  joint1Y = a1*sin(radians(theta1));
 
  joint2X = a1*cos(radians(theta1)) + a2*cos(radians(theta1+theta2));
  joint2Y = a1*sin(radians(theta1)) + a2*sin(radians(theta1+theta2));
  // github testing
}

Altre amb processing 2D i 3D amb tres articulacions simulaik-master.zip https://github.com/movilujo/simulaIK http://robotstyles.blogspot.com/2016/11/alli-esta-ahora-falta-descubrir-como.html?m=1

https://github.com/s4lt3d/IK_Sketch

vis_3d.pde

/**
 * Inverse Kinematics Demo, Walter Gordy 2014
*/
float[][] A1,A2,A3,A4,A5,A6,A7,T1,T2,T3,T4,T5,T6,T7; // matrix types
 
float theta1=0,theta2=0,theta3=0,theta4=0,theta5=0,theta6=0;
 
float a2 = 22.0;
float a3 = 0;
float d3 = 0;
float d4 = 17.0;
float tool = 15.5;
float px,py,pz;
float r11=1,r12=0,r13=0;
float r21=0,r22=1,r23=0;
float r31=0,r32=0,r33=1;
 
float hpx=0,hpy=-15,hpz=20;
float rx=0,ry=PI,rz=0;
 
float phpx = hpx;
float phpy = hpy;
float phpz = hpz;
 
float prx = rx;
float pry = ry;
float prz = rz;
 
void setup()
{
  size(850, 480, P3D);   
  A1 = matIdentity(4); // create new 4x4 matrix
  A2 = matIdentity(4); // create new 4x4 matrix
  A3 = matIdentity(4); // create new 4x4 matrix
  A4 = matIdentity(4); // create new 4x4 matrix
  A5 = matIdentity(4); // create new 4x4 matrix
  A6 = matIdentity(4); // create new 4x4 matrix
  A7 = matIdentity(4); // create new 4x4 matrix
}
 
void draw()
{ 
  //println("draw1");
  background(127);
 
  stroke(255);
  strokeWeight(0.1);
 
  if(mousePressed)
  {
 
  rx += (pmouseX - mouseX) * PI / width;
  ry += (pmouseY - mouseY) * PI / height;
  //rz += sliderRZ.getValue()/(PI*1000);
  }
  else
  {
   hpx = -(mouseX - width/2) * 30.0 / width; 
   hpz = (height - mouseY) * 30.0 / height;
  //hpz -= sliderZ.getValue()/1000;
 
  }
 
 
 
  //println("draw2");
  if(calculateIK() == false)
  {
    hpx = phpx;
    hpy = phpy;
    hpz = phpz;
 
    rx = prx;
    ry = pry;
    rz = prz; 
    calculateIK();
  }
  else
  {
 
  phpx = hpx;
  phpy = hpy;
  phpz = hpz;
 
  prx = rx;
  pry = ry;
  prz = rz; 
  }
 
  perspective();
 
  translate(width/2, height- height/3, 0);
 
  rotateX(PI/2);
 
  pushMatrix();
  scale(10);
 
  translate(-hpx,-hpy,hpz);
  fill(255,0,0);
  stroke(255,0,0);
  sphere(0.1);
  popMatrix();
  fill(255,255,255);
 
  pushMatrix();
  scale(10);
  stroke(0);
  fill(255);
  applyDHMatrix(A1);
  box(6,6,1);  
  applyDHMatrix(A2);
 
  pushMatrix();
  translate((a2)/2,0,0);
  box(a2-5,2,1);
  popMatrix();
 
  box(1,1,3);
  applyDHMatrix(A3);
  box(1,1,3);
 
  pushMatrix();
  translate(0,d4/2,0);
  box(2,d4-5,1);
  popMatrix();
 
  applyDHMatrix(A4);
  box(1,1,3);
  applyDHMatrix(A5);
  box(1,1,3);
  applyDHMatrix(A6);
//  box(1,1,3);
 
  pushMatrix();
  translate(0,0,tool/2);
  box(1,1,tool-5);
  popMatrix();
 
  applyDHMatrix(A7);
  fill(255,0,0);
  box(3,6,0.5);
 
  popMatrix();
  stroke(255);
 
}
 
boolean calculateIK()
{
  //println("calc1");
 
  float[][] rotation = rotationMatrix(rx,ry,rz);
 
 
  r11 = rotation[0][0];
  r12 = rotation[0][1];
  r13 = rotation[0][2];
  r21 = rotation[1][0];
  r22 = rotation[1][1];
  r23 = rotation[1][2];
  r31 = rotation[2][0];
  r32 = rotation[2][1];
  r33 = rotation[2][2];  
 
  px = hpx + tool * r13;
  py = hpy + tool * r23;
  pz = hpz - tool * r33;
 
 
  theta1 = atan2(py,px) - atan2(d3,sqrt(px*px + py*py - d3*d3));
 
  float K = (px*px + py*py + pz*pz - a2*a2 - a3*a3 - d3*d3 - d4*d4) / (2 * a2);
 
  theta3 = atan2(a3,d4) - atan2(K,-sqrt(a3*a3 + d4*d4 - K*K));
 
  float theta23 = atan2((-a3-a2*cos(theta3))*pz - (cos(theta1)*px + sin(theta1)*py)*(a2*sin(theta3) - d4),
                  (a2*sin(theta3)-d4)*pz - (a3+a2*cos(theta3))*(cos(theta1)*px + sin(theta1)*py));
  theta2 = theta23 - theta3;
 
  theta4 = atan2(-r13*sin(theta1)+r23*cos(theta1),-r13*cos(theta1)*cos(theta2+theta3) - r23*sin(theta1)*cos(theta2 + theta3) + r33*sin(theta2 + theta3));
 
  float s5 = -(r13*(cos(theta1)*cos(theta2 + theta3)*cos(theta4) +sin(theta1)*sin(theta4)) + r23*(sin(theta1)*cos(theta2+theta3)*cos(theta4) - cos(theta1)*sin(theta4)) - r33*(sin(theta2 + theta3)*cos(theta4)));
  float c5 = r13*(-cos(theta1)*sin(theta2 + theta3)) + r23*(-sin(theta1)*sin(theta2+theta3)) + r33*(-cos(theta2 + theta3));
 
  theta5 = atan2(s5,c5);
 
  float s6 = -r11*(cos(theta1)*cos(theta2 + theta3)*sin(theta4) - sin(theta1)*cos(theta4)) - r21*(sin(theta1)*cos(theta2 + theta3)*sin(theta4) + cos(theta1)*cos(theta4)) + r31*(sin(theta2 + theta3)*sin(theta4));
  float c6 = r11*((cos(theta1)*cos(theta2+theta3)*cos(theta4) + sin(theta1)*sin(theta4))*cos(theta5) - cos(theta1)*sin(theta2+theta3)*sin(theta5)) + r21*((sin(theta1)*cos(theta2+theta3)*cos(theta4) - cos(theta1)*sin(theta4))*cos(theta5) - sin(theta1)*sin(theta2+theta3)*sin(theta5))-r31*(sin(theta2+theta3)*cos(theta4)*cos(theta5) + cos(theta2+theta3)*sin(theta5));
 
  theta6 = atan2(s6,c6);
 
 // //println(px + "\t\t" + py + "\t\t" + pz + "\t\t" + atan2(d3,sqrt(px*px + py*py - d3*d3)));
  //println("calc2"); 
  A1 = DHMatrix(A1, 0,0,0,theta1);     // calculate initial DH Matrix
  A2 = DHMatrix(A2, 0,-PI/2,0,theta2);
  A3 = DHMatrix(A3, a2,0,0,theta3);
  A4 = DHMatrix(A4, 0,-PI/2,d4,theta4);
  A5 = DHMatrix(A5, 0,PI/2,0,theta5);
  A6 = DHMatrix(A6, 0,-PI/2,0,theta6);
  A7 = DHMatrix(A7, 0,0,tool,0);
 
  T1  = A1;                    // Calculate Positions
  T2 = matMultiply(T1,A2);
  T3 = matMultiply(T2,A3);
  T4 = matMultiply(T3,A4);
  T5 = matMultiply(T4,A5);
  T6 = matMultiply(T5,A6);
  T7 = matMultiply(T6,A7);
  //println("calc3");
 
  for(int i = 0; i < 3; i++)
  {
    //println("calc4");
    for(int j = 0; j < 3; j++)
    {
      //println("calc5");
      if(T7[i][j] == Float.NaN )
      {
         //println("calc6");
         return false;
      } 
 
      //println(T7[i][j]);
    } 
  }
 
  return true;
 
}
 
float[][] DHMatrix(float[][] Ai, float a_ , float alpha_ , float d_ , float theta_ )
{
  Ai[0][0] = cos(theta_);
  Ai[0][1] = -sin(theta_);
  Ai[0][2] = 0;
  Ai[0][3] = a_ ;
 
  Ai[1][0] = sin(theta_) * cos(alpha_);
  Ai[1][1] = cos(theta_) * cos(alpha_); 
  Ai[1][2] = -sin(alpha_);
  Ai[1][3] = -sin(alpha_) * d_;
 
  Ai[2][0] = sin(theta_) * sin(alpha_);
  Ai[2][1] = cos(theta_) * sin(alpha_);
  Ai[2][2] = cos(alpha_);
  Ai[2][3] = cos(alpha_) * d_;
 
  Ai[3][0] = 0;
  Ai[3][1] = 0;
  Ai[3][2] = 0;
  Ai[3][3] = 1; 
 
  return Ai;
 
}
 
void applyDHMatrix(float[][] Ai)
{
  applyMatrix(Ai[0][0],Ai[0][1],Ai[0][2], Ai[0][3],
              Ai[1][0],Ai[1][1],Ai[1][2], Ai[1][3],
              Ai[2][0],Ai[2][1],Ai[2][2], Ai[2][3],
              Ai[3][0],Ai[3][1],Ai[3][2], Ai[3][3]);
}
 
float[][] rotationMatrix(float rx, float ry, float rz)
{  
  float[][] Rx = matIdentity(3);
  float[][] Ry = matIdentity(3);
  float[][] Rz = matIdentity(3);
 
  Rx[1][1] = cos(rx);
  Rx[1][2] = -sin(rx);
  Rx[2][2] = cos(rx);
  Rx[2][1] = sin(rx);
 
  Ry[0][0] = cos(ry);
  Ry[2][2] = cos(ry);
  Ry[2][0] = -sin(ry);
  Ry[0][2] = sin(ry);
 
  Rz[0][0] = cos(rz);
  Rz[1][1] = cos(rz);
  Rz[0][1] = -sin(rz);
  Rz[1][0] = sin(rz);
 
  return matMultiply(Rx,matMultiply(Ry,Rz));
}
 
 
 
float[][] matMultiply(float a[][], float b[][]){//a[m][n], b[n][p]
   if(a.length == 0) return new float[0][0];
   if(a[0].length != b.length) return null; //invalid dims
 
   int n = a[0].length;
   int m = a.length;
   int p = b[0].length;
 
   float ans[][] = new float[m][p];
 
   for(int i = 0;i < m;i++){
      for(int j = 0;j < p;j++){
         for(int k = 0;k < n;k++){
            ans[i][j] += a[i][k] * b[k][j];
         }
      }
   }
   return ans;
}
 
 
float[][] matIdentity(int n)
{
   float[][] A = new float[n][n];
   for(int i = 0; i < n; i++)
     for(int j = 0; j < n; j++)
        if(i == j)
           A[i][j] = 1;
        else
           A[i][j] = 0;
   return A;
}