Quantcast
Channel: Library Questions - Processing 2.x and 3.x Forum
Viewing all articles
Browse latest Browse all 2896

Code crashes - Library missing? But which one?

$
0
0

Hey guys :) I'm trying to run a code to receive and visualize OSC Data from a nice IMU and UWB positioning system called POZYX. They have a processing sketch to visualize the capabilities of the system and I have problems to run it.

This is what happens:

pozyx orientation 3d

It would be amazing if you could help me out with that issue

Thanks already :) Cheers

And here is the code:

https://drive.google.com/file/d/0BxEoKClmcnO7M0JuQ2NmUWdjdTg/view?usp=sharing

import oscP5.*;
import org.gwoptics.graphics.graph2D.Graph2D;
import org.gwoptics.graphics.graph2D.traces.*;
import org.gwoptics.graphics.graph2D.backgrounds.*;
import org.gwoptics.graphics.GWColour;
import processing.serial.*;
import java.lang.Math.*;

boolean serial = false;          // set to true to use Serial, false to use OSC messages.

int oscPort = 8888;               // change this to your UDP port
String serialPort = "COM13";      // change this to your COM port


/////////////////////////////////////////////////////////////
//////////////////////  variables //////////////////////////
/////////////////////////////////////////////////////////////

OscP5 oscP5;
Serial myPort;

int     lf = 10;       //ASCII linefeed
String  inString;      //String for testing serial communication
int[] rgb_color = {0, 0, 255, 0, 160, 122, 0, 255, 0, 255};

Graph2D g_acc, g_gyro, g_mag;
PImage compass_img;

/////////////////////////////////////////////////////////////
///////////// sensordata variables //////////////////////////
/////////////////////////////////////////////////////////////

float x_angle = 0;
float y_angle = 0;
float z_angle = 0;

float speed_x = 0;
float speed_y = 0;
float speed_z = 0;

float lin_acc_x = 0;
float lin_acc_y = 0;
float lin_acc_z = 0;

float quat_w, quat_x, quat_y, quat_z;
float grav_x, grav_y, grav_z;
float heading = 0;
float pressure = 0;

String calib_status = "";

// array of sensor data over multiple timesteps
ArrayList<rangeData> accData;
ArrayList<rangeData> magData;
ArrayList<rangeData> gyroData;


/////////////////////////////////////////////////////////////
///////// class needed for the timeseries graph /////////////
/////////////////////////////////////////////////////////////

class rangeData implements ILine2DEquation{
    private double curVal = 0;

    public void setCurVal(double curVal) {
      this.curVal = curVal;
    }

    public double getCurVal() {
      return this.curVal;
    }

    public double computePoint(double x,int pos) {
      return curVal;
    }
}



void setup(){
  size(1100,800, P3D);
  surface.setResizable(true);
  stroke(0,0,0);
  colorMode(RGB, 256);

  compass_img = loadImage("compass.png");

  if(serial){
    try{
      myPort = new Serial(this, serialPort, 115200);
      myPort.clear();
      myPort.bufferUntil(lf);
    }catch(Exception e){
      println("Cannot open serial port.");
    }
  }else{
    try{
      oscP5 = new OscP5(this, oscPort);
    }catch(Exception e){
      println("Cannot open UDP port");
    }
  }


  // initialize running traces
  g_acc = new Graph2D(this, 400, 200, false);
  g_mag = new Graph2D(this, 400, 200, false);
  g_gyro = new Graph2D(this, 400, 200, false);

  accData = new ArrayList<rangeData>();
  magData = new ArrayList<rangeData>();
  gyroData = new ArrayList<rangeData>();
  for(int i=0; i<3; i++){
    rangeData r = new rangeData();
    accData.add(r);
    RollingLine2DTrace rl = new RollingLine2DTrace(r ,100,0.1f);
    rl.setTraceColour(rgb_color[i%10], rgb_color[(i+1)%10], rgb_color[(i+2)%10]);
    rl.setLineWidth(2);
    g_acc.addTrace(rl);

    r = new rangeData();
    magData.add(r);
    rl = new RollingLine2DTrace(r ,100,0.1f);
    rl.setTraceColour(rgb_color[i%10], rgb_color[(i+1)%10], rgb_color[(i+2)%10]);
    rl.setLineWidth(2);
    g_mag.addTrace(rl);

    r = new rangeData();
    gyroData.add(r);
    rl = new RollingLine2DTrace(r ,100,0.1f);
    rl.setTraceColour(rgb_color[i%10], rgb_color[(i+1)%10], rgb_color[(i+2)%10]);
    rl.setLineWidth(2);
    g_gyro.addTrace(rl);

  }

  // create the accelerometer graph
  g_acc.setYAxisMin(-2.0f);
  g_acc.setYAxisMax(2.0f);
  g_acc.position.y = 50;
  g_acc.position.x = 100;
  g_acc.setYAxisTickSpacing(0.5f);
  g_acc.setXAxisMax(5f);
  g_acc.setXAxisLabel("time (s)");
  g_acc.setYAxisLabel("acceleration [g]");
  g_acc.setBackground(new SolidColourBackground(new GWColour(1f,1f,1f)));

  // create the magnetometer graph
  g_mag.setYAxisMin(-80.0f);
  g_mag.setYAxisMax(80.0f);
  g_mag.position.y = 300;
  g_mag.position.x = 100;
  g_mag.setYAxisTickSpacing(40f);
  g_mag.setXAxisMax(5f);
  g_mag.setXAxisLabel("time (s)");
  g_mag.setYAxisLabel("magnetic field strength [µT]");
  g_mag.setBackground(new SolidColourBackground(new GWColour(1f,1f,1f)));

  // create the gyrometer graph
  g_gyro.setYAxisMin(-1000.0f);
  g_gyro.setYAxisMax(1000.0f);
  g_gyro.position.y = 550;
  g_gyro.position.x = 100;
  g_gyro.setYAxisTickSpacing(250f);
  g_gyro.setXAxisMax(5f);
  g_gyro.setXAxisLabel("time (s)");
  g_gyro.setYAxisLabel("angular velocity [deg/s]");
  g_gyro.setBackground(new SolidColourBackground(new GWColour(1f,1f,1f)));

}

void draw(){
    background(126,161,172);

    // show some text
    fill(0,0,0);
    text("(c) Pozyx Labs", width-100, 20);
    text("Calibration status:", 550, 730);
    text(calib_status, 550, 750);

    text("Pressure: " + pressure + "Pa", 550, 710);

    // draw the 3 graphs
    g_acc.draw();
    g_mag.draw();
    g_gyro.draw();

    //Show 3D orientation data
    stroke(0,0,0);
    strokeWeight(0.01);

    pushMatrix();
    translate(800, 500, -50);

    rotateX(radians(-90));
    rotateZ(radians(90));
    quat_rotate(quat_w, quat_x, quat_y, quat_z);

    // draw the 3D box
    draw_rect(93, 175, 83);

    // draw lines
    strokeWeight(0.1);
    line(0,0,0, grav_x*2, grav_y*2, grav_z*2);
    stroke(200,0,0);
    line(0,0,0, 0, 0, 1);

    // end rotation
    popMatrix();

    // show the linear acceleration in body coordinates
    int x_center = 700, y_center = 150;
    stroke(0,0,0);
    strokeWeight(1);
    line(x_center-75, y_center, x_center+75, y_center);
    line(x_center, y_center-75, x_center, y_center+75);
    ellipseMode(CENTER);
    ellipse(x_center, y_center, 50, 50);
    ellipse(x_center+lin_acc_x/50, y_center-lin_acc_y/50, 50*(1-lin_acc_z/1000), 50*(1-lin_acc_z/1000));
    fill(0,0,0);
    text("Linear acceleration\n(body coordinates)", x_center-50, y_center + 100);

    // show the linear acceleration in world coordinates
    x_center = 900;
    fill(93, 175, 83);
    stroke(0,0,0);
    strokeWeight(1);
    line(x_center-75, y_center, x_center+75, y_center);
    line(x_center, y_center-75, x_center, y_center+75);
    ellipseMode(CENTER);
    ellipse(x_center, y_center, 50, 50);

    PVector lin_acc = new PVector(lin_acc_x, lin_acc_y, lin_acc_z);
    //PVector lin_acc = new PVector(grav_x*1000.0f, grav_y*1000.0f, grav_z*1000.0f);    // test to verify the rotation
    lin_acc = quaternion_rotate(quat_w, quat_x, quat_y, quat_z, lin_acc);
    lin_acc.y = -lin_acc.y;
    lin_acc.z = -lin_acc.z;


    ellipseMode(CENTER);
    ellipse(x_center+lin_acc.x/50, y_center-lin_acc.y/50, 50*(1-lin_acc.z/1000), 50*(1-lin_acc.z/1000));
    fill(0,0,0);
    text("Linear acceleration\n(world coordinates)", x_center-50, y_center + 100);
    /*
    text("x: " + grav_x*1000.0f , 550, 310);
    text("y: " + grav_y*1000.0f , 550, 330);
    text("z: " + grav_z*1000.0f , 550, 350);

    text("x: " + lin_acc_x , 550, 310);
    text("y: " + lin_acc_y , 550, 330);
    text("z: " + lin_acc_z , 550, 350);

    text("x: " + lin_acc.x , 650, 310);
    text("y: " + lin_acc.y , 650, 330);
    text("z: " + lin_acc.z , 650, 350);
    */

    // draw the heading (compass)
    int img_size = 160;
    image(compass_img, 1000-img_size/2, 700-img_size/2, img_size, img_size);
    stroke(255,0,0);
    strokeWeight(3);
    line(1000, 700, 1000+50*cos(radians(heading)), 700+50*sin(radians(heading)));



}


void serialEvent(Serial p) {

  inString = (myPort.readString());
  println(inString);

  try {
    //Parse the data
    String[] dataStrings = split(inString, ',');

    // the pressure from mPa to Pa is coming in at a slower rate
    pressure = float(dataStrings[1])/1000.0f;

    // acceleration from mg to g
    accData.get(0).setCurVal(float(dataStrings[2])/1000.0f);
    accData.get(1).setCurVal(float(dataStrings[3])/1000.0f);
    accData.get(2).setCurVal(float(dataStrings[4])/1000.0f);

    // magnetometer data in µT
    magData.get(0).setCurVal(float(dataStrings[5])/16.0f);
    magData.get(1).setCurVal(float(dataStrings[6])/16.0f);
    magData.get(2).setCurVal(float(dataStrings[7])/16.0f);

    // gyroscope data in degrees per second
    gyroData.get(0).setCurVal(float(dataStrings[8])/16.0f);
    gyroData.get(1).setCurVal(float(dataStrings[9])/16.0f);
    gyroData.get(2).setCurVal(float(dataStrings[10])/16.0f);

    // Euler angles in degrees
    x_angle = float(dataStrings[13])/16.0f;
    y_angle = float(dataStrings[12])/16.0f;
    z_angle = float(dataStrings[11])/16.0f;
    heading = float(dataStrings[11])/16.0f;

    // the orientation quaternion
    quat_w = float(dataStrings[14])/16384.0f;
    quat_x = float(dataStrings[15])/16384.0f;
    quat_y = float(dataStrings[16])/16384.0f;
    quat_z = float(dataStrings[17])/16384.0f;
    float norm = PApplet.sqrt(quat_x * quat_x + quat_y * quat_y + quat_z
                * quat_z +quat_w * quat_w);
    quat_w = quat_w/norm;
    quat_x = quat_x/norm;
    quat_y = quat_y/norm;
    quat_z = quat_z/norm;
    println(norm);

    // linear acceleration in mg
    lin_acc_x = float(dataStrings[18]);
    lin_acc_y = float(dataStrings[19]);
    lin_acc_z = float(dataStrings[20]);

    // gravitation vector from mg to g
    grav_x = float(dataStrings[21])/1000.0f;
    grav_y = float(dataStrings[22])/1000.0f;
    grav_z = float(dataStrings[23])/1000.0f;

    // the calibration status
    calib_status = "Mag: " + dataStrings[24] + " - Acc: " + dataStrings[25] + " - Gyro: " + dataStrings[26] + " - System: " + dataStrings[27];

  } catch (Exception e) {
      println("Error while reading serial data.");
  }
}

void oscEvent(OscMessage theOscMessage) {
  // osc message received
  println("### received an osc message with addrpattern "+theOscMessage.addrPattern()+" and typetag "+theOscMessage.typetag());
  if (theOscMessage.addrPattern().equals("/sensordata")){
    //theOscMessage.print();}
    try{
      // the pressure from mPa to Pa is coming in at a slower rate
      pressure = theOscMessage.get(1).floatValue();

      // acceleration from mg to g
      accData.get(0).setCurVal(theOscMessage.get(2).floatValue()/1000.0f);
      accData.get(1).setCurVal(theOscMessage.get(3).floatValue()/1000.0f);
      accData.get(2).setCurVal(theOscMessage.get(4).floatValue()/1000.0f);

      // magnetometer data in µT
      magData.get(0).setCurVal(theOscMessage.get(5).floatValue());
      magData.get(1).setCurVal(theOscMessage.get(6).floatValue());
      magData.get(2).setCurVal(theOscMessage.get(7).floatValue());

      // gyroscope data in degrees per second
      gyroData.get(0).setCurVal(theOscMessage.get(8).floatValue());
      gyroData.get(1).setCurVal(theOscMessage.get(9).floatValue());
      gyroData.get(2).setCurVal(theOscMessage.get(10).floatValue());

      // Euler angles in degrees
      x_angle = theOscMessage.get(13).floatValue();
      y_angle = theOscMessage.get(12).floatValue();
      z_angle = theOscMessage.get(11).floatValue();
      heading = theOscMessage.get(11).floatValue();

      // the orientation quaternion
      quat_w = theOscMessage.get(14).floatValue();
      quat_x = theOscMessage.get(15).floatValue();
      quat_y = theOscMessage.get(16).floatValue();
      quat_z = theOscMessage.get(17).floatValue();
      float norm = PApplet.sqrt(quat_x * quat_x + quat_y * quat_y + quat_z
                  * quat_z +quat_w * quat_w);
      quat_w = quat_w/norm;
      quat_x = quat_x/norm;
      quat_y = quat_y/norm;
      quat_z = quat_z/norm;
      println(norm);

      // linear acceleration in mg
      lin_acc_x = theOscMessage.get(18).floatValue();
      lin_acc_y = theOscMessage.get(19).floatValue();
      lin_acc_z = theOscMessage.get(20).floatValue();

      // gravitation vector from mg to g
      grav_x = theOscMessage.get(21).floatValue();
      grav_y = theOscMessage.get(22).floatValue();
      grav_z = theOscMessage.get(23).floatValue();

      // the calibration status
      calib_status = "Mag: " + str(theOscMessage.get(24).intValue()) + " - Acc: " + str(theOscMessage.get(25).intValue()) + " - Gyro: " + str(theOscMessage.get(26).intValue()) + " - System: " + str(theOscMessage.get(27).intValue());
    }catch(Exception e){
      println("Error while receiving OSC sensor data");
    }
  }
}


void draw_rect(int r, int g, int b) {
  scale(100);
  beginShape(QUADS);

  fill(r, g, b);
  vertex(-1,  1.5,  0.25);
  vertex( 1,  1.5,  0.25);
  vertex( 1, -1.5,  0.25);
  vertex(-1, -1.5,  0.25);

  vertex( 1,  1.5,  0.25);
  vertex( 1,  1.5, -0.25);
  vertex( 1, -1.5, -0.25);
  vertex( 1, -1.5,  0.25);

  vertex( 1,  1.5, -0.25);
  vertex(-1,  1.5, -0.25);
  vertex(-1, -1.5, -0.25);
  vertex( 1, -1.5, -0.25);

  vertex(-1,  1.5, -0.25);
  vertex(-1,  1.5,  0.25);
  vertex(-1, -1.5,  0.25);
  vertex(-1, -1.5, -0.25);

  vertex(-1,  1.5, -0.25);
  vertex( 1,  1.5, -0.25);
  vertex( 1,  1.5,  0.25);
  vertex(-1,  1.5,  0.25);

  vertex(-1, -1.5, -0.25);
  vertex( 1, -1.5, -0.25);
  vertex( 1, -1.5,  0.25);
  vertex(-1, -1.5,  0.25);

  endShape();

}

public void quat_rotate(float w, float x, float y, float z) {
   float _x, _y, _z;
   //if (q1.w > 1) q1.normalise(); // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is normalised
   double angle = 2 * Math.acos(w);
   float s = (float)Math.sqrt(1-w*w); // assuming quaternion normalised then w is less than 1, so term always positive.
   if (s < 0.001) { // test to avoid divide by zero, s is always positive due to sqrt
     // if s close to zero then direction of axis not important
     _x = x; // if it is important that axis is normalised then replace with x=1; y=z=0;
     _y = y;
     _z = z;
   } else {
     _x = x / s; // normalise axis
     _y = y / s;
     _z = z / s;
   }
   rotate((float)angle, _x, _y, _z);
}

public final PVector quaternion_rotate(float w, float x, float y, float z, PVector v) {

      float q00 = 2.0f * x * x;
      float q11 = 2.0f * y * y;
      float q22 = 2.0f * z * z;

      float q01 = 2.0f * x * y;
      float q02 = 2.0f * x * z;
      float q03 = 2.0f * x * w;

      float q12 = 2.0f * y * z;
      float q13 = 2.0f * y * w;

      float q23 = 2.0f * z * w;

      return new PVector((1.0f - q11 - q22) * v.x + (q01 - q23) * v.y
                      + (q02 + q13) * v.z, (q01 + q23) * v.x + (1.0f - q22 - q00) * v.y
                  + (q12 - q03) * v.z, (q02 - q13) * v.x + (q12 + q03) * v.y
                  + (1.0f - q11 - q00) * v.z);

}


Viewing all articles
Browse latest Browse all 2896

Trending Articles