Help with double spherical pendulum animation

Processing Libraries
1

Hey guys,

I’m kinda new to processing and I’m struggling on a project that I’m working on. My project (as already stated in the title) is programming and animating a double spherical pendulum.

I used Mathematica to solve (using the Runge-Kutta method) the equations for the values that I need (the theta and phi angles of each pendulum bob). And I have verification that the equations are correct and do work.

This leads me to the conclusion that my code is all funky and that’s why I’m here. Soo here is my code, can anyone help me?

import peasy.PeasyCam; // Camera for movement (variable for camera is cam)
PeasyCam cam;

// variables for maths
float l1 = 250;
float l2 = 250;
float m1 = 25;
float m2 = 25;
//angles
float theta1 = PI/16;
float phi1 = PI/4;
float theta2 = PI/4;
float phi2 = PI/24;
//velocities
float theta1d = 0;
float phi1d = 0;
float theta2d = 0;
float phi2d = 0;
//accelerations
float theta1dd = 0;
float phi1dd = 0;
float theta2dd = 0;
float phi2dd = 0;
//storage for new angles, didn't end up using, may use for later so don't delete
//float newtheta1 = 0;
//float newphi1 = 0;
//float newtheta2 = 0;
//float newphi2 = 0;

float g = 9.807;
float i; // storage variable to stop draw loop





void settings(){
  size(900, 900, P3D);
  //smooth();
}

void setup() {
  cam = new PeasyCam(this,2000); // camera 1000 pixels away from origin along
  //camera(0,0,500, 0,0,0,-1,-1,-1); don't use built in camera


}


void draw(){ 
  scale(1,-1,1); // make into cartesian coordinates for easier maths
  background(0);
  strokeWeight(4);
  
  
  // A  X   I   S
  //Y - RED
  stroke(255,0,0);
  line(0,0,0,0,500,0);
  stroke(150,0,0);
  line(0,0,0,0,-500,0);
  

  
  //X - GREEN
  stroke(0,255,0);
  line(0,0,0,500,0,0);
  stroke(0,150,0);
  line(0,0,0,-500,0,0);
  
  //Z - BLUE
  stroke(0,0,255);
  line(0,0,0,0,0,500);
  stroke(0,0, 150);
  line(0,0,0,0,0,-500);
  
  stroke(255);
  strokeWeight(2);
  
  
  //lights();  //light to make it look cool
  //rotateZ(i+=0.012);    
  cam.setYawRotationMode();  // Permit pich/roll only.
    
  //float t = 1/frameRate;

  //float distance = pow((pow((x2-x1), 2))+pow((y2-y1) ,2)+pow((z2-z1), 2), 0.5);
    
  cam.beginHUD();
  //debug printing
  textSize(25);
  fill(255, 255, 255);
  text("theta1dd =" + theta1dd, 10, 85);
  text("phi1dd   =" + phi1dd, 10, 115);
  text("theta2dd =" + theta2dd, 10, 145);
  text("phi2dd   =" + phi2dd, 10, 175);   
  cam.endHUD();

  
  float x1 = l1*sin(theta1)*cos(phi1);
  float z1 = l1*sin(theta1)*sin(phi1);
  float y1 = -l1*cos(theta1);
  float x2 = x1 + l2*sin(theta2)*cos(phi2);
  float z2 = z1 + l2*sin(theta2)*sin(phi2);    
  float y2 = y1 - l2 * cos(theta2);
  
  // FIND all the dd values and iterate them down to the theta level. done
  phi1dd    = //equations. there were too many characters so i had to delete;
  phi2dd    = //I can send equations to anyone who needs them;
  theta1dd  = //;
  theta2dd  = //;
  
  // STORE new theta in new variable. 
  //CHANGING THE VALUE 
  //theta  = t * w0   +   t^2 * 0.5 * angular acceleration
  //t = 1 frame its simplified to w0 + 0.5 * angular acceleration 

  theta1d   = theta1d + 0.5 * theta1dd;
  theta1 = theta1 + theta1d;
 
  phi1d     = phi1d + 0.5 * phi1dd;
  phi1      = phi1  + phi1d;
  
  theta2d   = theta2d + 0.5*theta2dd;
  theta2    = theta2  + theta2d;
  
  phi2d     = phi2d + 0.5*phi2dd;
  phi2      = phi2  + phi2d;

Currently I am using peasyCam for viewing the simulation. Also, I plan to use picking library later on to be able to move the pendulum bobs with the mouse.

Sorry that I just copy and pasted my entire code. I just signed up so i don’t really know how to format stuff :frowning:

1 Like
2

In your code the end seems to be missing.

That’s the last line:

phi2      = phi2  + phi2d;
1 Like
3

ah yes,

it does end properly end in my code, sorry :slight_smile:

4

For those of you wanting the equations, i had to split them into 2 comments


phi1dd    = -((cos(theta2)*(l2*(pow(m2,2))*(pow(theta2d,2))*(1/tan(phi1-phi2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(1/sin(theta1))-2*l1*(pow(m2,2))*phi1d*theta1d*(pow((1/tan(theta1)),3))*(1/cos(theta2))-4*l1*(pow(m2,2))*phi1d*theta1d*(pow((1/tan(phi1-phi2)),2))*(pow((1/tan(theta1)),3))*(1/cos(theta2))-2*l1*(pow(m2,2))*phi1d*theta1d*(pow((1/tan(phi1-phi2)),4))*(pow((1/tan(theta1)),3))*(1/cos(theta2))-l1*m1*m2*(pow(phi1d,2))*(1/tan(phi1-phi2))*(pow((1/tan(theta1)),2))*(pow((1/sin(phi1-phi2)),2))*(1/cos(theta2))-l1*(pow(m2,2))*(pow(phi1d,2))*(1/tan(phi1-phi2))*(pow((1/tan(theta1)),2))*(pow((1/sin(phi1-phi2)),2))*(1/cos(theta2))+2*l1*m1*m2*phi1d*theta1d*(pow((1/tan(phi1-phi2)),2))*(pow((1/tan(theta1)),3))*(pow((1/sin(phi1-phi2)),2))*(1/cos(theta2))+2*l1*(pow(m2,2))*phi1d*theta1d*(pow((1/tan(phi1-phi2)),2))*(pow((1/tan(theta1)),3))*(pow((1/sin(phi1-phi2)),2))*(1/cos(theta2))+g*m1*m2*(1/tan(phi1-phi2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(1/sin(theta1))*(1/cos(theta2))+g*(pow(m2,2))*(1/tan(phi1-phi2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(1/sin(theta1))*(1/cos(theta2))+l1*m1*m2*(pow(phi1d,2))*(1/tan(phi1-phi2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(1/cos(theta2))+l1*(pow(m2,2))*(pow(phi1d,2))*(1/tan(phi1-phi2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(1/cos(theta2))+l1*m1*m2*(pow(theta1d,2))*(1/tan(phi1-phi2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(1/cos(theta2))+l1*(pow(m2,2))*(pow(theta1d,2))*(1/tan(phi1-phi2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(1/cos(theta2))+2*l1*m1*m2*phi1d*theta1d*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(1/cos(theta2))+2*l1*(pow(m2,2))*phi1d*theta1d*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(1/cos(theta2))-l2*(pow(m2,2))*(pow(theta2d,2))*(1/tan(phi1-phi2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(1/sin(theta1))*(pow((1/cos(theta2)),2))+l1*m1*m2*(pow(phi1d,2))*(1/tan(phi1-phi2))*(pow((1/tan(theta1)),2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/cos(theta2)),3))+l1*(pow(m2,2))*(pow(phi1d,2))*(1/tan(phi1-phi2))*(pow((1/tan(theta1)),2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/cos(theta2)),3))+2*l1*m1*m2*phi1d*theta1d*(pow((1/tan(theta1)),3))*(pow((1/sin(phi1-phi2)),2))*(pow((1/cos(theta2)),3))+2*l1*(pow(m2,2))*phi1d*theta1d*(pow((1/tan(theta1)),3))*(pow((1/sin(phi1-phi2)),2))*(pow((1/cos(theta2)),3))-g*m1*m2*(1/tan(phi1-phi2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(1/sin(theta1))*(pow((1/cos(theta2)),3))-g*(pow(m2,2))*(1/tan(phi1-phi2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(1/sin(theta1))*(pow((1/cos(theta2)),3))-l1*m1*m2*(pow(phi1d,2))*(1/tan(phi1-phi2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),3))-l1*(pow(m2,2))*(pow(phi1d,2))*(1/tan(phi1-phi2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),3))-l1*m1*m2*(pow(theta1d,2))*(1/tan(phi1-phi2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),3))-l1*(pow(m2,2))*(pow(theta1d,2))*(1/tan(phi1-phi2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),3))+2*l1*m1*m2*phi1d*theta1d*(pow((1/tan(phi1-phi2)),2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),3))+2*l1*(pow(m2,2))*phi1d*theta1d*(pow((1/tan(phi1-phi2)),2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),3))-2*l1*(pow(m1,2))*phi1d*theta1d*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),4))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),3))-4*l1*m1*m2*phi1d*theta1d*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),4))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),3))-2*l1*(pow(m2,2))*phi1d*theta1d*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),4))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),3))-l2*(pow(m2,2))*(pow(phi2d,2))*(pow((1/tan(theta1)),2))*(1/sin(phi1-phi2))*(1/sin(theta1))*tan(theta2)-l2*(pow(m2,2))*(pow(phi2d,2))*(pow((1/tan(phi1-phi2)),2))*(pow((1/tan(theta1)),2))*(1/sin(phi1-phi2))*(1/sin(theta1))*tan(theta2)+l2*(pow(m2,2))*(pow(theta2d,2))*(pow((1/sin(phi1-phi2)),3))*(1/sin(theta1))*tan(theta2)+l2*m1*m2*(pow(phi2d,2))*(pow((1/sin(phi1-phi2)),3))*(pow((1/sin(theta1)),3))*tan(theta2)+l2*(pow(m2,2))*(pow(phi2d,2))*(pow((1/sin(phi1-phi2)),3))*(pow((1/sin(theta1)),3))*tan(theta2)+l1*(pow(m2,2))*(pow(phi1d,2))*(1/tan(theta1))*(1/sin(phi1-phi2))*(1/cos(theta2))*tan(theta2)+l1*(pow(m2,2))*(pow(theta1d,2))*(1/tan(theta1))*(1/sin(phi1-phi2))*(1/cos(theta2))*tan(theta2)+l1*(pow(m2,2))*(pow(phi1d,2))*(pow((1/tan(phi1-phi2)),2))*(1/tan(theta1))*(1/sin(phi1-phi2))*(1/cos(theta2))*tan(theta2)+l1*(pow(m2,2))*(pow(theta1d,2))*(pow((1/tan(phi1-phi2)),2))*(1/tan(theta1))*(1/sin(phi1-phi2))*(1/cos(theta2))*tan(theta2)-4*l1*(pow(m2,2))*phi1d*theta1d*(1/tan(phi1-phi2))*(pow((1/tan(theta1)),2))*(1/sin(phi1-phi2))*(1/cos(theta2))*tan(theta2)-4*l1*(pow(m2,2))*phi1d*theta1d*(pow((1/tan(phi1-phi2)),3))*(pow((1/tan(theta1)),2))*(1/sin(phi1-phi2))*(1/cos(theta2))*tan(theta2)+l1*(pow(m2,2))*(pow(theta1d,2))*(pow((1/tan(theta1)),3))*(1/sin(phi1-phi2))*(1/cos(theta2))*tan(theta2)+l1*(pow(m2,2))*(pow(theta1d,2))*(pow((1/tan(phi1-phi2)),2))*(pow((1/tan(theta1)),3))*(1/sin(phi1-phi2))*(1/cos(theta2))*tan(theta2)-l1*m1*m2*(pow(phi1d,2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),3))*(1/cos(theta2))*tan(theta2)-l1*(pow(m2,2))*(pow(phi1d,2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),3))*(1/cos(theta2))*tan(theta2)+4*l1*m1*m2*phi1d*theta1d*(1/tan(phi1-phi2))*(pow((1/tan(theta1)),2))*(pow((1/sin(phi1-phi2)),3))*(1/cos(theta2))*tan(theta2)+4*l1*(pow(m2,2))*phi1d*theta1d*(1/tan(phi1-phi2))*(pow((1/tan(theta1)),2))*(pow((1/sin(phi1-phi2)),3))*(1/cos(theta2))*tan(theta2)+g*(pow(m2,2))*(pow((1/tan(theta1)),2))*(1/sin(phi1-phi2))*(1/sin(theta1))*(1/cos(theta2))*tan(theta2)+g*(pow(m2,2))*(pow((1/tan(phi1-phi2)),2))*(pow((1/tan(theta1)),2))*(1/sin(phi1-phi2))*(1/sin(theta1))*(1/cos(theta2))*tan(theta2)+g*m1*m2*(pow((1/sin(phi1-phi2)),3))*(1/sin(theta1))*(1/cos(theta2))*tan(theta2)+g*(pow(m2,2))*(pow((1/sin(phi1-phi2)),3))*(1/sin(theta1))*(1/cos(theta2))*tan(theta2)-l1*m1*m2*(pow(theta1d,2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),3))*(pow((1/sin(theta1)),2))*(1/cos(theta2))*tan(theta2)-l1*(pow(m2,2))*(pow(theta1d,2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),3))*(pow((1/sin(theta1)),2))*(1/cos(theta2))*tan(theta2)-g*m1*m2*(pow((1/sin(phi1-phi2)),3))*(pow((1/sin(theta1)),3))*(1/cos(theta2))*tan(theta2)-g*(pow(m2,2))*(pow((1/sin(phi1-phi2)),3))*(pow((1/sin(theta1)),3))*(1/cos(theta2))*tan(theta2)+l2*(pow(m2,2))*(pow(phi2d,2))*(pow((1/tan(theta1)),2))*(1/sin(phi1-phi2))*(1/sin(theta1))*(pow((1/cos(theta2)),2))*tan(theta2)+l2*(pow(m2,2))*(pow(theta2d,2))*(pow((1/tan(theta1)),2))*(1/sin(phi1-phi2))*(1/sin(theta1))*(pow((1/cos(theta2)),2))*tan(theta2)+l2*(pow(m2,2))*(pow(phi2d,2))*(pow((1/tan(phi1-phi2)),2))*(pow((1/tan(theta1)),2))*(1/sin(phi1-phi2))*(1/sin(theta1))*(pow((1/cos(theta2)),2))*tan(theta2)+l2*(pow(m2,2))*(pow(theta2d,2))*(pow((1/tan(phi1-phi2)),2))*(pow((1/tan(theta1)),2))*(1/sin(phi1-phi2))*(1/sin(theta1))*(pow((1/cos(theta2)),2))*tan(theta2)-l2*m1*m2*(pow(phi2d,2))*(pow((1/sin(phi1-phi2)),3))*(pow((1/sin(theta1)),3))*(pow((1/cos(theta2)),2))*tan(theta2)-l2*(pow(m2,2))*(pow(phi2d,2))*(pow((1/sin(phi1-phi2)),3))*(pow((1/sin(theta1)),3))*(pow((1/cos(theta2)),2))*tan(theta2)-l2*m1*m2*(pow(theta2d,2))*(pow((1/sin(phi1-phi2)),3))*(pow((1/sin(theta1)),3))*(pow((1/cos(theta2)),2))*tan(theta2)-l2*(pow(m2,2))*(pow(theta2d,2))*(pow((1/sin(phi1-phi2)),3))*(pow((1/sin(theta1)),3))*(pow((1/cos(theta2)),2))*tan(theta2)+l2*(pow(m2,2))*(pow(theta2d,2))*(1/tan(phi1-phi2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(1/sin(theta1))*(pow(tan(theta2),2))+l1*(pow(m2,2))*(pow(phi1d,2))*(1/tan(phi1-phi2))*(pow((1/sin(phi1-phi2)),2))*(1/cos(theta2))*(pow(tan(theta2),2))+l1*(pow(m2,2))*(pow(theta1d,2))*(1/tan(phi1-phi2))*(pow((1/sin(phi1-phi2)),2))*(1/cos(theta2))*(pow(tan(theta2),2))-2*l1*(pow(m2,2))*phi1d*theta1d*(pow((1/tan(phi1-phi2)),2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(1/cos(theta2))*(pow(tan(theta2),2))+l1*(pow(m2,2))*(pow(theta1d,2))*(1/tan(phi1-phi2))*(pow((1/tan(theta1)),2))*(pow((1/sin(phi1-phi2)),2))*(1/cos(theta2))*(pow(tan(theta2),2))+2*l1*m1*m2*phi1d*theta1d*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),4))*(1/cos(theta2))*(pow(tan(theta2),2))+2*l1*(pow(m2,2))*phi1d*theta1d*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),4))*(1/cos(theta2))*(pow(tan(theta2),2))+g*(pow(m2,2))*(1/tan(phi1-phi2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),2))*(1/sin(theta1))*(1/cos(theta2))*(pow(tan(theta2),2))+l2*(pow(m2,2))*(pow(phi2d,2))*(pow((1/sin(phi1-phi2)),3))*(1/sin(theta1))*(pow(tan(theta2),3))+l2*(pow(m2,2))*(pow(theta2d,2))*(pow((1/sin(phi1-phi2)),3))*(1/sin(theta1))*(pow(tan(theta2),3))))/(l1*(-(pow(m2,2))*(pow((1/tan(theta1)),2))-2*(pow(m2,2))*(pow((1/tan(phi1-phi2)),2))*(pow((1/tan(theta1)),2))-(pow(m2,2))*(pow((1/tan(phi1-phi2)),4))*(pow((1/tan(theta1)),2))+m1*m2*(pow((1/tan(phi1-phi2)),2))*(pow((1/tan(theta1)),2))*(pow((1/sin(phi1-phi2)),2))+(pow(m2,2))*(pow((1/tan(phi1-phi2)),2))*(pow((1/tan(theta1)),2))*(pow((1/sin(phi1-phi2)),2))+m1*m2*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))+(pow(m2,2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))+m1*m2*(pow((1/tan(theta1)),2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/cos(theta2)),2))+(pow(m2,2))*(pow((1/tan(theta1)),2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/cos(theta2)),2))+m1*m2*(pow((1/tan(phi1-phi2)),2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),2))+(pow(m2,2))*(pow((1/tan(phi1-phi2)),2))*(pow((1/sin(phi1-phi2)),2))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),2))-(pow(m1,2))*(pow((1/sin(phi1-phi2)),4))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),2))-2*m1*m2*(pow((1/sin(phi1-phi2)),4))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),2))-(pow(m2,2))*(pow((1/sin(phi1-phi2)),4))*(pow((1/sin(theta1)),2))*(pow((1/cos(theta2)),2))-2*(pow(m2,2))*(1/tan(phi1-phi2))*(1/tan(theta1))*(1/sin(phi1-phi2))*tan(theta2)-2*(pow(m2,2))*(pow((1/tan(phi1-phi2)),3))*(1/tan(theta1))*(1/sin(phi1-phi2))*tan(theta2)+2*m1*m2*(1/tan(phi1-phi2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),3))*tan(theta2)+2*(pow(m2,2))*(1/tan(phi1-phi2))*(1/tan(theta1))*(pow((1/sin(phi1-phi2)),3))*tan(theta2)-(pow(m2,2))*(pow((1/tan(phi1-phi2)),2))*(pow((1/sin(phi1-phi2)),2))*(pow(tan(theta2),2))+m1*m2*(pow((1/sin(phi1-phi2)),4))*(pow(tan(theta2),2))+(pow(m2,2))*(pow((1/sin(phi1-phi2)),4))*(pow(tan(theta2),2)))));
  phi2dd    = -((cos(theta2)*(2*l2*m1*m2*phi2d*theta2d*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))+2*l2*(pow(m2,2))*phi2d*theta2d*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))-2*l2*(pow(m2,2))*phi2d*theta2d*(pow(cos(phi1-phi2),4))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))-2*l2*(pow(m1,2))*phi2d*theta2d*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),2))-4*l2*m1*m2*phi2d*theta2d*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),2))-2*l2*(pow(m2,2))*phi2d*theta2d*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),2))+2*l2*m1*m2*phi2d*theta2d*(pow(cos(phi1-phi2),2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),2))+2*l2*(pow(m2,2))*phi2d*theta2d*(pow(cos(phi1-phi2),2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),2))-l2*m1*m2*(pow(phi2d,2))*cos(phi1-phi2)*(pow((1/sin(theta2)),2))*(1/cos(theta2))*sin(phi1-phi2)-l2*(pow(m2,2))*(pow(phi2d,2))*cos(phi1-phi2)*(pow((1/sin(theta2)),2))*(1/cos(theta2))*sin(phi1-phi2)+l2*m1*m2*(pow(phi2d,2))*cos(phi1-phi2)*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))*(1/cos(theta2))*sin(phi1-phi2)+l2*(pow(m2,2))*(pow(phi2d,2))*cos(phi1-phi2)*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))*(1/cos(theta2))*sin(phi1-phi2)+g*m1*m2*cos(phi1-phi2)*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)+g*(pow(m2,2))*cos(phi1-phi2)*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)+l1*m1*m2*(pow(theta1d,2))*cos(phi1-phi2)*cos(theta1)*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)+l1*(pow(m2,2))*(pow(theta1d,2))*cos(phi1-phi2)*cos(theta1)*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)-g*m1*m2*cos(phi1-phi2)*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)-g*(pow(m2,2))*cos(phi1-phi2)*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)-l1*m1*m2*(pow(theta1d,2))*cos(phi1-phi2)*(pow(cos(theta1),3))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)-l1*(pow(m2,2))*(pow(theta1d,2))*cos(phi1-phi2)*(pow(cos(theta1),3))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)+l2*m1*m2*(pow(phi2d,2))*cos(phi1-phi2)*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)+l2*(pow(m2,2))*(pow(phi2d,2))*cos(phi1-phi2)*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)+l2*m1*m2*(pow(theta2d,2))*cos(phi1-phi2)*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)+l2*(pow(m2,2))*(pow(theta2d,2))*cos(phi1-phi2)*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)-l2*m1*m2*(pow(phi2d,2))*cos(phi1-phi2)*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)-l2*(pow(m2,2))*(pow(phi2d,2))*cos(phi1-phi2)*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)-l2*m1*m2*(pow(theta2d,2))*cos(phi1-phi2)*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)-l2*(pow(m2,2))*(pow(theta2d,2))*cos(phi1-phi2)*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)+2*l2*m1*m2*phi2d*theta2d*(pow((1/sin(theta2)),3))*(pow(sin(phi1-phi2),2))+2*l2*(pow(m2,2))*phi2d*theta2d*(pow((1/sin(theta2)),3))*(pow(sin(phi1-phi2),2))-4*l2*(pow(m2,2))*phi2d*theta2d*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))*(pow(sin(phi1-phi2),2))+2*l2*m1*m2*phi2d*theta2d*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),2))*(pow(sin(phi1-phi2),2))+2*l2*(pow(m2,2))*phi2d*theta2d*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),2))*(pow(sin(phi1-phi2),2))-2*l2*(pow(m2,2))*phi2d*theta2d*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))*(pow(sin(phi1-phi2),4))+4*l2*m1*m2*phi2d*theta2d*cos(phi1-phi2)*cos(theta1)*(pow((1/sin(theta2)),2))*(1/cos(theta2))*sin(theta1)+4*l2*(pow(m2,2))*phi2d*theta2d*cos(phi1-phi2)*cos(theta1)*(pow((1/sin(theta2)),2))*(1/cos(theta2))*sin(theta1)-4*l2*(pow(m2,2))*phi2d*theta2d*(pow(cos(phi1-phi2),3))*cos(theta1)*(pow((1/sin(theta2)),2))*(1/cos(theta2))*sin(theta1)-l2*(pow(m2,2))*(pow(theta2d,2))*(pow(cos(phi1-phi2),2))*cos(theta1)*(pow((1/sin(theta2)),3))*sin(phi1-phi2)*sin(theta1)-l1*m1*m2*(pow(phi1d,2))*(pow(cos(phi1-phi2),2))*(pow((1/sin(theta2)),3))*(1/cos(theta2))*sin(phi1-phi2)*sin(theta1)-l1*(pow(m2,2))*(pow(phi1d,2))*(pow(cos(phi1-phi2),2))*(pow((1/sin(theta2)),3))*(1/cos(theta2))*sin(phi1-phi2)*sin(theta1)-l1*m1*m2*(pow(theta1d,2))*(pow(cos(phi1-phi2),2))*(pow((1/sin(theta2)),3))*(1/cos(theta2))*sin(phi1-phi2)*sin(theta1)-l1*(pow(m2,2))*(pow(theta1d,2))*(pow(cos(phi1-phi2),2))*(pow((1/sin(theta2)),3))*(1/cos(theta2))*sin(phi1-phi2)*sin(theta1)-g*m1*m2*(pow(cos(phi1-phi2),2))*cos(theta1)*(pow((1/sin(theta2)),3))*(1/cos(theta2))*sin(phi1-phi2)*sin(theta1)-g*(pow(m2,2))*(pow(cos(phi1-phi2),2))*cos(theta1)*(pow((1/sin(theta2)),3))*(1/cos(theta2))*sin(phi1-phi2)*sin(theta1)+l1*m1*m2*(pow(phi1d,2))*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))*(1/cos(theta2))*sin(phi1-phi2)*sin(theta1)+l1*(pow(m2,2))*(pow(phi1d,2))*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))*(1/cos(theta2))*sin(phi1-phi2)*sin(theta1)+l2*m1*m2*(pow(phi2d,2))*cos(theta1)*(1/sin(theta2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)*sin(theta1)+l2*(pow(m2,2))*(pow(phi2d,2))*cos(theta1)*(1/sin(theta2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)*sin(theta1)-l2*(pow(m2,2))*(pow(phi2d,2))*(pow(cos(phi1-phi2),2))*cos(theta1)*(1/sin(theta2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)*sin(theta1)-l2*(pow(m2,2))*(pow(theta2d,2))*(pow(cos(phi1-phi2),2))*cos(theta1)*(1/sin(theta2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)*sin(theta1)+l2*m1*m2*(pow(theta2d,2))*cos(theta1)*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)*sin(theta1)+l2*(pow(m2,2))*(pow(theta2d,2))*cos(theta1)*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)*sin(theta1)-g*m1*m2*cos(theta1)*(1/sin(theta2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)-g*(pow(m2,2))*cos(theta1)*(1/sin(theta2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)-l1*m1*m2*(pow(theta1d,2))*(pow(cos(theta1),2))*(1/sin(theta2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)-l1*(pow(m2,2))*(pow(theta1d,2))*(pow(cos(theta1),2))*(1/sin(theta2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)+l1*(pow(m1,2))*(pow(phi1d,2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)+2*l1*m1*m2*(pow(phi1d,2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)+l1*(pow(m2,2))*(pow(phi1d,2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)+l1*(pow(m1,2))*(pow(theta1d,2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)+2*l1*m1*m2*(pow(theta1d,2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)+l1*(pow(m2,2))*(pow(theta1d,2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)+g*(pow(m1,2))*cos(theta1)*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)+2*g*m1*m2*cos(theta1)*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)+g*(pow(m2,2))*cos(theta1)*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)-l1*(pow(m1,2))*(pow(phi1d,2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)-2*l1*m1*m2*(pow(phi1d,2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)-l1*(pow(m2,2))*(pow(phi1d,2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*sin(theta1)-4*l2*(pow(m2,2))*phi2d*theta2d*cos(phi1-phi2)*cos(theta1)*(pow((1/sin(theta2)),2))*(1/cos(theta2))*(pow(sin(phi1-phi2),2))*sin(theta1)-l2*(pow(m2,2))*(pow(theta2d,2))*cos(theta1)*(pow((1/sin(theta2)),3))*(pow(sin(phi1-phi2),3))*sin(theta1)-l1*m1*m2*(pow(phi1d,2))*(pow((1/sin(theta2)),3))*(1/cos(theta2))*(pow(sin(phi1-phi2),3))*sin(theta1)-l1*(pow(m2,2))*(pow(phi1d,2))*(pow((1/sin(theta2)),3))*(1/cos(theta2))*(pow(sin(phi1-phi2),3))*sin(theta1)-l1*m1*m2*(pow(theta1d,2))*(pow((1/sin(theta2)),3))*(1/cos(theta2))*(pow(sin(phi1-phi2),3))*sin(theta1)-l1*(pow(m2,2))*(pow(theta1d,2))*(pow((1/sin(theta2)),3))*(1/cos(theta2))*(pow(sin(phi1-phi2),3))*sin(theta1)-g*m1*m2*cos(theta1)*(pow((1/sin(theta2)),3))*(1/cos(theta2))*(pow(sin(phi1-phi2),3))*sin(theta1)-g*(pow(m2,2))*cos(theta1)*(pow((1/sin(theta2)),3))*(1/cos(theta2))*(pow(sin(phi1-phi2),3))*sin(theta1)+l1*m1*m2*(pow(phi1d,2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))*(1/cos(theta2))*(pow(sin(phi1-phi2),3))*sin(theta1)+l1*(pow(m2,2))*(pow(phi1d,2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),3))*(1/cos(theta2))*(pow(sin(phi1-phi2),3))*sin(theta1)-l2*(pow(m2,2))*(pow(phi2d,2))*cos(theta1)*(1/sin(theta2))*(pow((1/cos(theta2)),2))*(pow(sin(phi1-phi2),3))*sin(theta1)-l2*(pow(m2,2))*(pow(theta2d,2))*cos(theta1)*(1/sin(theta2))*(pow((1/cos(theta2)),2))*(pow(sin(phi1-phi2),3))*sin(theta1)+2*l2*m1*m2*phi2d*theta2d*(1/sin(theta2))*(pow((1/cos(theta2)),2))*(pow(sin(theta1),2))+2*l2*(pow(m2,2))*phi2d*theta2d*(1/sin(theta2))*(pow((1/cos(theta2)),2))*(pow(sin(theta1),2))-2*l2*(pow(m2,2))*phi2d*theta2d*(pow(cos(phi1-phi2),2))*(1/sin(theta2))*(pow((1/cos(theta2)),2))*(pow(sin(theta1),2))-l2*(pow(m2,2))*(pow(theta2d,2))*cos(phi1-phi2)*(pow((1/sin(theta2)),2))*(1/cos(theta2))*sin(phi1-phi2)*(pow(sin(theta1),2))-g*m1*m2*cos(phi1-phi2)*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)*(pow(sin(theta1),2))-g*(pow(m2,2))*cos(phi1-phi2)*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)*(pow(sin(theta1),2))-l1*m1*m2*(pow(theta1d,2))*cos(phi1-phi2)*cos(theta1)*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)*(pow(sin(theta1),2))-l1*(pow(m2,2))*(pow(theta1d,2))*cos(phi1-phi2)*cos(theta1)*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*sin(phi1-phi2)*(pow(sin(theta1),2))-l2*(pow(m2,2))*(pow(phi2d,2))*cos(phi1-phi2)*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*(pow(sin(theta1),2))-l2*(pow(m2,2))*(pow(theta2d,2))*cos(phi1-phi2)*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*(pow(sin(theta1),2))-l1*m1*m2*(pow(phi1d,2))*(1/sin(theta2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*(pow(sin(theta1),3))-l1*(pow(m2,2))*(pow(phi1d,2))*(1/sin(theta2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*(pow(sin(theta1),3))-l1*m1*m2*(pow(theta1d,2))*(1/sin(theta2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*(pow(sin(theta1),3))-l1*(pow(m2,2))*(pow(theta1d,2))*(1/sin(theta2))*(pow((1/cos(theta2)),3))*sin(phi1-phi2)*(pow(sin(theta1),3))))/(l2*(m1*m2*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))+(pow(m2,2))*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))-(pow(m2,2))*(pow(cos(phi1-phi2),4))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))-(pow(m1,2))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))-2*m1*m2*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))-(pow(m2,2))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))+m1*m2*(pow(cos(phi1-phi2),2))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))+(pow(m2,2))*(pow(cos(phi1-phi2),2))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))+m1*m2*(pow((1/sin(theta2)),2))*(pow(sin(phi1-phi2),2))+(pow(m2,2))*(pow((1/sin(theta2)),2))*(pow(sin(phi1-phi2),2))-2*(pow(m2,2))*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))*(pow(sin(phi1-phi2),2))+m1*m2*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*(pow(sin(phi1-phi2),2))+(pow(m2,2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))*(pow((1/cos(theta2)),2))*(pow(sin(phi1-phi2),2))-(pow(m2,2))*(pow(cos(theta1),2))*(pow((1/sin(theta2)),2))*(pow(sin(phi1-phi2),4))+2*m1*m2*cos(phi1-phi2)*cos(theta1)*(1/sin(theta2))*(1/cos(theta2))*sin(theta1)+2*(pow(m2,2))*cos(phi1-phi2)*cos(theta1)*(1/sin(theta2))*(1/cos(theta2))*sin(theta1)-2*(pow(m2,2))*(pow(cos(phi1-phi2),3))*cos(theta1)*(1/sin(theta2))*(1/cos(theta2))*sin(theta1)-2*(pow(m2,2))*cos(phi1-phi2)*cos(theta1)*(1/sin(theta2))*(1/cos(theta2))*(pow(sin(phi1-phi2),2))*sin(theta1)+m1*m2*(pow((1/cos(theta2)),2))*(pow(sin(theta1),2))+(pow(m2,2))*(pow((1/cos(theta2)),2))*(pow(sin(theta1),2))-(pow(m2,2))*(pow(cos(phi1-phi2),2))*(pow((1/cos(theta2)),2))*(pow(sin(theta1),2)))));
5

second set of equations:

theta1dd  = -(((pow(l1, 2)*(pow(l2,4))*(pow(m2,3))*cos(phi1-phi2)*cos(theta1)*sin(phi1-phi2)*sin(theta1)*(pow(sin(theta2),2))-(pow(l1,2))*(pow(l2,4))*(pow(m2,3))*cos(theta2)*sin(phi1-phi2)*sin(theta1)*(pow(sin(theta2),2))*(cos(phi1-phi2)*cos(theta1)*cos(theta2)+sin(theta1)*sin(theta2)))*(-(pow(l1,2)*(pow(l2,2))*m2*(m1+m2)*(pow(sin(theta1),2))*(pow(sin(theta2),2))-(pow(l1,2))*(pow(l2,2))*(pow(m2,2))*(pow(cos(phi1-phi2),2))*(pow(sin(theta1),2))*(pow(sin(theta2),2)))*(2*l1*l2*m2*phi1d*theta1d*cos(theta1)*cos(theta2)*sin(phi1-phi2)+l1*l2*m2*(pow(phi1d,2))*cos(phi1-phi2)*cos(theta2)*sin(theta1)-g*l2*m2*sin(theta2)+(pow(l2,2))*m2*(pow(phi2d,2))*cos(theta2)*sin(theta2)+l1*l2*m2*(pow(theta1d,2))*(cos(phi1-phi2)*cos(theta2)*sin(theta1)-cos(theta1)*sin(theta2)))+l1*l2*m2*cos(theta2)*sin(phi1-phi2)*sin(theta1)*(-l1*l2*m2*cos(phi1-phi2)*sin(theta1)*sin(theta2)*(2*l1*l2*m2*phi1d*theta1d*cos(phi1-phi2)*cos(theta1)*sin(theta2)+2*(pow(l2,2))*m2*phi2d*theta2d*cos(theta2)*sin(theta2)-l1*l2*m2*(pow(phi1d,2)+(pow(theta1d,2)))*sin(phi1-phi2)*sin(theta1)*sin(theta2))+(pow(l2,2))*m2*(pow(sin(theta2),2))*(2*(pow(l1,2))*(m1+m2)*phi1d*theta1d*cos(theta1)*sin(theta1)+2*l1*l2*m2*phi2d*theta2d*cos(phi1-phi2)*cos(theta2)*sin(theta1)+l1*l2*m2*(pow(phi2d,2)+(pow(theta2d,2)))*sin(phi1-phi2)*sin(theta1)*sin(theta2))))-(-(pow(l1,2))*(pow(l2,4))*(pow(m2,3))*(pow(cos(theta2),2))*(pow(sin(phi1-phi2),2))*(pow(sin(theta1),2))*(pow(sin(theta2),2))+(pow(l2,2))*m2*(pow(l1,2)*(pow(l2,2))*m2*(m1+m2)*(pow(sin(theta1),2))*(pow(sin(theta2),2))-(pow(l1,2))*(pow(l2,2))*(pow(m2,2))*(pow(cos(phi1-phi2),2))*(pow(sin(theta1),2))*(pow(sin(theta2),2))))*(-(pow(l1,2))*(pow(l2,2))*(pow(m2,2))*cos(phi1-phi2)*cos(theta1)*sin(phi1-phi2)*sin(theta1)*(pow(sin(theta2),2))*(2*l1*l2*m2*phi1d*theta1d*cos(theta1)*cos(theta2)*sin(phi1-phi2)+l1*l2*m2*(pow(phi1d,2))*cos(phi1-phi2)*cos(theta2)*sin(theta1)-g*l2*m2*sin(theta2)+(pow(l2,2))*m2*(pow(phi2d,2))*cos(theta2)*sin(theta2)+l1*l2*m2*(pow(theta1d,2))*(cos(phi1-phi2)*cos(theta2)*sin(theta1)-cos(theta1)*sin(theta2)))+l1*l2*m2*cos(theta2)*sin(phi1-phi2)*sin(theta1)*(l1*l2*m2*cos(theta1)*sin(phi1-phi2)*sin(theta2)*(2*l1*l2*m2*phi1d*theta1d*cos(phi1-phi2)*cos(theta1)*sin(theta2)+2*(pow(l2,2))*m2*phi2d*theta2d*cos(theta2)*sin(theta2)-l1*l2*m2*(pow(phi1d,2)+(pow(theta1d,2)))*sin(phi1-phi2)*sin(theta1)*sin(theta2))+(pow(l2,2))*m2*(pow(sin(theta2),2))*(-2*l1*l2*m2*phi2d*theta2d*cos(theta1)*cos(theta2)*sin(phi1-phi2)-g*l1*(m1+m2)*sin(theta1)+(pow(l1,2))*(m1+m2)*(pow(phi1d,2))*cos(theta1)*sin(theta1)+l1*l2*m2*(pow(phi2d,2))*cos(phi1-phi2)*cos(theta1)*sin(theta2)-l1*l2*m2*(pow(theta2d,2))*(cos(theta2)*sin(theta1)-cos(phi1-phi2)*cos(theta1)*sin(theta2))))))/(-(pow(l1,3)*(pow(l2,3))*(pow(m2,3))*cos(phi1-phi2)*cos(theta1)*sin(phi1-phi2)*sin(theta1)*(pow(sin(theta2),2))*(cos(phi1-phi2)*cos(theta1)*cos(theta2)+sin(theta1)*sin(theta2))+l1*l2*m2*cos(theta2)*sin(phi1-phi2)*sin(theta1)*(pow(l2,2)*m2*(-(pow(l1,2))*m1-(pow(l1,2))*m2)*(pow(sin(theta2),2))+(pow(l1,2))*(pow(l2,2))*(pow(m2,2))*(pow(cos(theta1),2))*(pow(sin(phi1-phi2),2))*(pow(sin(theta2),2))))*(-(pow(l1,2))*(pow(l2,4))*(pow(m2,3))*(pow(cos(theta2),2))*(pow(sin(phi1-phi2),2))*(pow(sin(theta1),2))*(pow(sin(theta2),2))+(pow(l2,2))*m2*(pow(l1,2)*(pow(l2,2))*m2*(m1+m2)*(pow(sin(theta1),2))*(pow(sin(theta2),2))-(pow(l1,2))*(pow(l2,2))*(pow(m2,2))*(pow(cos(phi1-phi2),2))*(pow(sin(theta1),2))*(pow(sin(theta2),2))))+(pow(l1,2)*(pow(l2,4))*(pow(m2,3))*cos(phi1-phi2)*cos(theta1)*sin(phi1-phi2)*sin(theta1)*(pow(sin(theta2),2))-(pow(l1,2))*(pow(l2,4))*(pow(m2,3))*cos(theta2)*sin(phi1-phi2)*sin(theta1)*(pow(sin(theta2),2))*(cos(phi1-phi2)*cos(theta1)*cos(theta2)+sin(theta1)*sin(theta2)))*(-(pow(l1,3))*(pow(l2,3))*(pow(m2,3))*cos(phi1-phi2)*cos(theta1)*cos(theta2)*(pow(sin(phi1-phi2),2))*(pow(sin(theta1),2))*(pow(sin(theta2),2))+l1*l2*m2*(cos(phi1-phi2)*cos(theta1)*cos(theta2)+sin(theta1)*sin(theta2))*(pow(l1,2)*(pow(l2,2))*m2*(m1+m2)*(pow(sin(theta1),2))*(pow(sin(theta2),2))-(pow(l1,2))*(pow(l2,2))*(pow(m2,2))*(pow(cos(phi1-phi2),2))*(pow(sin(theta1),2))*(pow(sin(theta2),2))))));
  theta2dd  = -((-l1*(pow(m1,2))*(pow(phi1d,2))*cos(phi1-phi2)*cos(theta2)*sin(theta1)-2*l1*m1*m2*(pow(phi1d,2))*cos(phi1-phi2)*cos(theta2)*sin(theta1)-l1*(pow(m2,2))*(pow(phi1d,2))*cos(phi1-phi2)*cos(theta2)*sin(theta1)-l1*(pow(m1,2))*(pow(theta1d,2))*cos(phi1-phi2)*cos(theta2)*sin(theta1)-2*l1*m1*m2*(pow(theta1d,2))*cos(phi1-phi2)*cos(theta2)*sin(theta1)-l1*(pow(m2,2))*(pow(theta1d,2))*cos(phi1-phi2)*cos(theta2)*sin(theta1)+l1*m1*m2*(pow(phi1d,2))*(pow(cos(phi1-phi2),3))*cos(theta2)*sin(theta1)+l1*(pow(m2,2))*(pow(phi1d,2))*(pow(cos(phi1-phi2),3))*cos(theta2)*sin(theta1)+l1*m1*m2*(pow(theta1d,2))*(pow(cos(phi1-phi2),3))*cos(theta2)*sin(theta1)+l1*(pow(m2,2))*(pow(theta1d,2))*(pow(cos(phi1-phi2),3))*cos(theta2)*sin(theta1)-g*(pow(m1,2))*cos(phi1-phi2)*cos(theta1)*cos(theta2)*sin(theta1)-2*g*m1*m2*cos(phi1-phi2)*cos(theta1)*cos(theta2)*sin(theta1)-g*(pow(m2,2))*cos(phi1-phi2)*cos(theta1)*cos(theta2)*sin(theta1)+g*m1*m2*(pow(cos(phi1-phi2),3))*cos(theta1)*cos(theta2)*sin(theta1)+g*(pow(m2,2))*(pow(cos(phi1-phi2),3))*cos(theta1)*cos(theta2)*sin(theta1)+l1*(pow(m1,2))*(pow(phi1d,2))*cos(phi1-phi2)*(pow(cos(theta1),2))*cos(theta2)*sin(theta1)+2*l1*m1*m2*(pow(phi1d,2))*cos(phi1-phi2)*(pow(cos(theta1),2))*cos(theta2)*sin(theta1)+l1*(pow(m2,2))*(pow(phi1d,2))*cos(phi1-phi2)*(pow(cos(theta1),2))*cos(theta2)*sin(theta1)-l1*m1*m2*(pow(phi1d,2))*(pow(cos(phi1-phi2),3))*(pow(cos(theta1),2))*cos(theta2)*sin(theta1)-l1*(pow(m2,2))*(pow(phi1d,2))*(pow(cos(phi1-phi2),3))*(pow(cos(theta1),2))*cos(theta2)*sin(theta1)-l2*m1*m2*(pow(theta2d,2))*cos(phi1-phi2)*cos(theta1)*(pow(cos(theta2),2))*sin(theta1)-l2*(pow(m2,2))*(pow(theta2d,2))*cos(phi1-phi2)*cos(theta1)*(pow(cos(theta2),2))*sin(theta1)+l2*(pow(m2,2))*(pow(theta2d,2))*(pow(cos(phi1-phi2),3))*cos(theta1)*(pow(cos(theta2),2))*sin(theta1)+l1*m1*m2*(pow(phi1d,2))*cos(phi1-phi2)*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta1)+l1*(pow(m2,2))*(pow(phi1d,2))*cos(phi1-phi2)*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta1)+l1*m1*m2*(pow(theta1d,2))*cos(phi1-phi2)*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta1)+l1*(pow(m2,2))*(pow(theta1d,2))*cos(phi1-phi2)*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta1)+g*m1*m2*cos(phi1-phi2)*cos(theta1)*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta1)+g*(pow(m2,2))*cos(phi1-phi2)*cos(theta1)*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta1)-l1*m1*m2*(pow(phi1d,2))*cos(phi1-phi2)*(pow(cos(theta1),2))*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta1)-l1*(pow(m2,2))*(pow(phi1d,2))*cos(phi1-phi2)*(pow(cos(theta1),2))*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta1)+l2*(pow(m2,2))*(pow(theta2d,2))*cos(phi1-phi2)*cos(theta1)*(pow(cos(theta2),2))*(pow(sin(phi1-phi2),2))*sin(theta1)+g*(pow(m1,2))*sin(theta2)+2*g*m1*m2*sin(theta2)+g*(pow(m2,2))*sin(theta2)-g*m1*m2*(pow(cos(phi1-phi2),2))*sin(theta2)-g*(pow(m2,2))*(pow(cos(phi1-phi2),2))*sin(theta2)+l1*(pow(m1,2))*(pow(theta1d,2))*cos(theta1)*sin(theta2)+2*l1*m1*m2*(pow(theta1d,2))*cos(theta1)*sin(theta2)+l1*(pow(m2,2))*(pow(theta1d,2))*cos(theta1)*sin(theta2)-l1*m1*m2*(pow(theta1d,2))*(pow(cos(phi1-phi2),2))*cos(theta1)*sin(theta2)-l1*(pow(m2,2))*(pow(theta1d,2))*(pow(cos(phi1-phi2),2))*cos(theta1)*sin(theta2)-l2*(pow(m1,2))*(pow(phi2d,2))*cos(theta2)*sin(theta2)-2*l2*m1*m2*(pow(phi2d,2))*cos(theta2)*sin(theta2)-l2*(pow(m2,2))*(pow(phi2d,2))*cos(theta2)*sin(theta2)+l2*m1*m2*(pow(phi2d,2))*(pow(cos(phi1-phi2),2))*cos(theta2)*sin(theta2)+l2*(pow(m2,2))*(pow(phi2d,2))*(pow(cos(phi1-phi2),2))*cos(theta2)*sin(theta2)+l2*m1*m2*(pow(phi2d,2))*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*cos(theta2)*sin(theta2)+l2*(pow(m2,2))*(pow(phi2d,2))*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*cos(theta2)*sin(theta2)+l2*m1*m2*(pow(theta2d,2))*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*cos(theta2)*sin(theta2)+l2*(pow(m2,2))*(pow(theta2d,2))*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*cos(theta2)*sin(theta2)-l2*(pow(m2,2))*(pow(phi2d,2))*(pow(cos(phi1-phi2),4))*(pow(cos(theta1),2))*cos(theta2)*sin(theta2)-l2*(pow(m2,2))*(pow(theta2d,2))*(pow(cos(phi1-phi2),4))*(pow(cos(theta1),2))*cos(theta2)*sin(theta2)-g*m1*m2*(pow(cos(theta1),2))*(pow(sin(phi1-phi2),2))*sin(theta2)-g*(pow(m2,2))*(pow(cos(theta1),2))*(pow(sin(phi1-phi2),2))*sin(theta2)-l1*m1*m2*(pow(theta1d,2))*(pow(cos(theta1),3))*(pow(sin(phi1-phi2),2))*sin(theta2)-l1*(pow(m2,2))*(pow(theta1d,2))*(pow(cos(theta1),3))*(pow(sin(phi1-phi2),2))*sin(theta2)+l2*m1*m2*(pow(phi2d,2))*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta2)+l2*(pow(m2,2))*(pow(phi2d,2))*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta2)+l2*m1*m2*(pow(theta2d,2))*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta2)+l2*(pow(m2,2))*(pow(theta2d,2))*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta2)+l2*m1*m2*(pow(phi2d,2))*(pow(cos(theta1),2))*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta2)+l2*(pow(m2,2))*(pow(phi2d,2))*(pow(cos(theta1),2))*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta2)-2*l2*(pow(m2,2))*(pow(phi2d,2))*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta2)-2*l2*(pow(m2,2))*(pow(theta2d,2))*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta2)-l2*(pow(m2,2))*(pow(phi2d,2))*(pow(cos(theta1),2))*cos(theta2)*(pow(sin(phi1-phi2),4))*sin(theta2)-l2*(pow(m2,2))*(pow(theta2d,2))*(pow(cos(theta1),2))*cos(theta2)*(pow(sin(phi1-phi2),4))*sin(theta2)-g*(pow(m1,2))*(pow(sin(theta1),2))*sin(theta2)-2*g*m1*m2*(pow(sin(theta1),2))*sin(theta2)-g*(pow(m2,2))*(pow(sin(theta1),2))*sin(theta2)+g*m1*m2*(pow(cos(phi1-phi2),2))*(pow(sin(theta1),2))*sin(theta2)+g*(pow(m2,2))*(pow(cos(phi1-phi2),2))*(pow(sin(theta1),2))*sin(theta2)+l1*(pow(m1,2))*(pow(phi1d,2))*cos(theta1)*(pow(sin(theta1),2))*sin(theta2)+2*l1*m1*m2*(pow(phi1d,2))*cos(theta1)*(pow(sin(theta1),2))*sin(theta2)+l1*(pow(m2,2))*(pow(phi1d,2))*cos(theta1)*(pow(sin(theta1),2))*sin(theta2)-l1*m1*m2*(pow(phi1d,2))*(pow(cos(phi1-phi2),2))*cos(theta1)*(pow(sin(theta1),2))*sin(theta2)-l1*(pow(m2,2))*(pow(phi1d,2))*(pow(cos(phi1-phi2),2))*cos(theta1)*(pow(sin(theta1),2))*sin(theta2)-l2*m1*m2*(pow(theta2d,2))*cos(theta2)*(pow(sin(theta1),2))*sin(theta2)-l2*(pow(m2,2))*(pow(theta2d,2))*cos(theta2)*(pow(sin(theta1),2))*sin(theta2)+l2*(pow(m2,2))*(pow(theta2d,2))*(pow(cos(phi1-phi2),2))*cos(theta2)*(pow(sin(theta1),2))*sin(theta2)-l1*m1*m2*(pow(phi1d,2))*cos(theta1)*(pow(sin(phi1-phi2),2))*(pow(sin(theta1),2))*sin(theta2)-l1*(pow(m2,2))*(pow(phi1d,2))*cos(theta1)*(pow(sin(phi1-phi2),2))*(pow(sin(theta1),2))*sin(theta2)-l1*m1*m2*(pow(theta1d,2))*cos(theta1)*(pow(sin(phi1-phi2),2))*(pow(sin(theta1),2))*sin(theta2)-l1*(pow(m2,2))*(pow(theta1d,2))*cos(theta1)*(pow(sin(phi1-phi2),2))*(pow(sin(theta1),2))*sin(theta2)+l2*m1*m2*(pow(phi2d,2))*cos(phi1-phi2)*cos(theta1)*sin(theta1)*(pow(sin(theta2),2))+l2*(pow(m2,2))*(pow(phi2d,2))*cos(phi1-phi2)*cos(theta1)*sin(theta1)*(pow(sin(theta2),2))+l2*m1*m2*(pow(theta2d,2))*cos(phi1-phi2)*cos(theta1)*sin(theta1)*(pow(sin(theta2),2))+l2*(pow(m2,2))*(pow(theta2d,2))*cos(phi1-phi2)*cos(theta1)*sin(theta1)*(pow(sin(theta2),2))-l2*(pow(m2,2))*(pow(phi2d,2))*(pow(cos(phi1-phi2),3))*cos(theta1)*sin(theta1)*(pow(sin(theta2),2))-l2*(pow(m2,2))*(pow(theta2d,2))*(pow(cos(phi1-phi2),3))*cos(theta1)*sin(theta1)*(pow(sin(theta2),2))-l2*(pow(m2,2))*(pow(phi2d,2))*cos(phi1-phi2)*cos(theta1)*(pow(sin(phi1-phi2),2))*sin(theta1)*(pow(sin(theta2),2))-l2*(pow(m2,2))*(pow(theta2d,2))*cos(phi1-phi2)*cos(theta1)*(pow(sin(phi1-phi2),2))*sin(theta1)*(pow(sin(theta2),2)))/(l2*(pow(m1,2)+2*m1*m2+(pow(m2,2))-m1*m2*(pow(cos(phi1-phi2),2))-(pow(m2,2))*(pow(cos(phi1-phi2),2))-m1*m2*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*(pow(cos(theta2),2))-(pow(m2,2))*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*(pow(cos(theta2),2))+(pow(m2,2))*(pow(cos(phi1-phi2),4))*(pow(cos(theta1),2))*(pow(cos(theta2),2))-m1*m2*(pow(cos(theta1),2))*(pow(sin(phi1-phi2),2))-(pow(m2,2))*(pow(cos(theta1),2))*(pow(sin(phi1-phi2),2))-m1*m2*(pow(cos(theta2),2))*(pow(sin(phi1-phi2),2))-(pow(m2,2))*(pow(cos(theta2),2))*(pow(sin(phi1-phi2),2))+2*(pow(m2,2))*(pow(cos(phi1-phi2),2))*(pow(cos(theta1),2))*(pow(cos(theta2),2))*(pow(sin(phi1-phi2),2))+(pow(m2,2))*(pow(cos(theta1),2))*(pow(cos(theta2),2))*(pow(sin(phi1-phi2),4))-2*m1*m2*cos(phi1-phi2)*cos(theta1)*cos(theta2)*sin(theta1)*sin(theta2)-2*(pow(m2,2))*cos(phi1-phi2)*cos(theta1)*cos(theta2)*sin(theta1)*sin(theta2)+2*(pow(m2,2))*(pow(cos(phi1-phi2),3))*cos(theta1)*cos(theta2)*sin(theta1)*sin(theta2)+2*(pow(m2,2))*cos(phi1-phi2)*cos(theta1)*cos(theta2)*(pow(sin(phi1-phi2),2))*sin(theta1)*sin(theta2)-m1*m2*(pow(sin(theta1),2))*(pow(sin(theta2),2))-(pow(m2,2))*(pow(sin(theta1),2))*(pow(sin(theta2),2))+(pow(m2,2))*(pow(cos(phi1-phi2),2))*(pow(sin(theta1),2))*(pow(sin(theta2),2)))));
6

And how does the code end?

Is something drawn?

Or is only } missing?

7

yeah, it just ends with the }

8

@Chrisir I also put a delay(1000) before the } so i could see the change of the values in my gui more easily and try and figure to what was happening

9

Hello,

Your animation goes off into la la land (you will see this) after a while.

I did the following to explore and visualize this:

  • Changed g to a lower value; I used 1.

  • Assumed you were doing some plotting with line() or point() and added these.

  • I added this to slow things down:

*   if (frameCount%5 == 0)
*     {
*     theta1d   = theta1d + 0.5*theta1dd;
*     theta1 = theta1 + theta1d;
*    
*     phi1d     = phi1d + 0.5*phi1dd;
*     phi1      = phi1  + phi1d;
*     
*     theta2d   = theta2d + 0.5*theta2dd;
*     theta2    = theta2  + theta2d;
*     
*     phi2d     = phi2d + 0.5*phi2dd;
*     phi2      = phi2  + phi2d;
*     }
*     
*   line(0, 0, 0, x1, y1, z1);
*   line(x1, y1, z1, x2, y2, z2);
*   }

  • You can use a variation of this to start and stop you animation:
    https://processing.org/reference/noLoop_.html

  • Add some println() statements so you can review data in console.

  • Added your formulas.

  • I also tried converting all the math to double and then back to a float at the end for plotting.
    It still went off into la la land.

:)

1 Like
10

Hey man, so it still bugs out really bad?

hmmm…

yes, I do have a line thing written in and to works fine

also, I did use delay(1000) to slow the thing down

any idea why it’s behaviour is like this?