BodyPoseSequence.java
Go to the documentation of this file.
00001 package edu.tum.cs.ias.knowrob.vis.items;
00002 
00003 import java.io.BufferedReader;
00004 import java.io.File;
00005 import java.io.FileReader;
00006 import java.io.IOException;
00007 import java.util.ArrayList;
00008 
00009 import edu.tum.cs.ias.knowrob.util.datastructures.Point;
00010 import edu.tum.cs.ias.knowrob.vis.Canvas;
00011 import edu.tum.cs.ias.knowrob.vis.DrawableAnimated;
00012 
00013 import javax.vecmath.Vector3f;
00014 
00020 public class BodyPoseSequence extends ItemBase implements DrawableAnimated {
00021         
00022         protected ArrayList<Point[]> jointPositions = new ArrayList<Point[]>();
00023         protected Integer trackedBodyPart = null, trackedBodyPartStartTime;
00024         protected int color = 0xffff00ff;
00025         
00026         protected final float armsThickness = 0.035f;
00027         protected final float spineThickness = 0.04f;
00028         
00029         protected boolean wireMode = false;
00030 
00031         public void addPose(float EPISODE_NR, float TIME, String action, 
00032                           float BECX, float BECY, float BECZ,  float ULWX, float ULWY, float ULWZ, 
00033                           float OLWX, float OLWY, float OLWZ,  float UBWX, float UBWY, float UBWZ, 
00034                           float OBWX, float OBWY, float OBWZ,  float UHWX, float UHWY, float UHWZ,
00035                           float BRKX, float BRKY, float BRKZ,  float OHWX, float OHWY, float OHWZ,
00036                           float KOX,  float KOY,  float KOZ,   float SEHX, float SEHY, float SEHZ,
00037                           float OSLX, float OSLY, float OSLZ,  float USLX, float USLY, float USLZ, 
00038                           float FULX, float FULY, float FULZ,  float FBLX, float FBLY, float FBLZ,
00039                           float OSRX, float OSRY, float OSRZ,  float USRX, float USRY, float USRZ,
00040                           float FURX, float FURY, float FURZ,  float FBRX, float FBRY, float FBRZ,
00041                           float SBLX, float SBLY, float SBLZ,  float OALX, float OALY, float OALZ,
00042                           float UALX, float UALY, float UALZ,  float HALX, float HALY, float HALZ,
00043                           float FILX, float FILY, float FILZ,  float SBRX, float SBRY, float SBRZ,  
00044                           float OARX, float OARY, float OARZ,  float UARX, float UARY, float UARZ, 
00045                           float HARX, float HARY, float HARZ,  float FIRX, float FIRY, float FIRZ ){
00046                   
00047                   Point[] newPosition = new Point[28];
00048                  
00049                   newPosition[0]  = new Point(BECX, BECY, BECZ, EPISODE_NR, TIME, "BEC", action);         
00050                   newPosition[1]  = new Point(ULWX, ULWY, ULWZ, EPISODE_NR, TIME, "ULW", action);
00051                   newPosition[2]  = new Point(OLWX, OLWY, OLWZ, EPISODE_NR, TIME, "OLW", action);
00052                   newPosition[3]  = new Point(UBWX, UBWY, UBWZ, EPISODE_NR, TIME, "UBW", action);
00053                   newPosition[4]  = new Point(OBWX, OBWY, OBWZ, EPISODE_NR, TIME, "OBW", action);
00054                   newPosition[5]  = new Point(UHWX, UHWY, UHWZ, EPISODE_NR, TIME, "UHW", action);
00055                   newPosition[6]  = new Point(BRKX, BRKY, BRKZ, EPISODE_NR, TIME, "BRK", action);
00056                   newPosition[7]  = new Point(OHWX, OHWY, OHWZ, EPISODE_NR, TIME, "OHW", action);
00057                   newPosition[8]  = new Point(KOX,  KOY,  KOZ,  EPISODE_NR, TIME, "KO", action);
00058                   newPosition[9]  = new Point(SEHX, SEHY, SEHZ, EPISODE_NR, TIME, "SEH", action);
00059                   newPosition[10] = new Point(OSLX, OSLY, OSLZ, EPISODE_NR, TIME, "OSL", action);
00060                   newPosition[11] = new Point(USLX, USLY, USLZ, EPISODE_NR, TIME, "USL", action);
00061                   newPosition[12] = new Point(FULX, FULY, FULZ, EPISODE_NR, TIME, "FUL", action);
00062                   newPosition[13] = new Point(FBLX, FBLY, FBLZ, EPISODE_NR, TIME, "FBL", action);
00063                   newPosition[14] = new Point(OSRX, OSRY, OSRZ, EPISODE_NR, TIME, "OSR", action);
00064                   newPosition[15] = new Point(USRX, USRY, USRZ, EPISODE_NR, TIME, "USR", action);
00065                   newPosition[16] = new Point(FURX, FURY, FURZ, EPISODE_NR, TIME, "FUR", action);
00066                   newPosition[17] = new Point(FBRX, FBRY, FBRZ, EPISODE_NR, TIME, "FBR", action);
00067                   newPosition[18] = new Point(SBLX, SBLY, SBLZ, EPISODE_NR, TIME, "SBL", action);
00068                   newPosition[19] = new Point(OALX, OALY, OALZ, EPISODE_NR, TIME, "OAL", action);
00069                   newPosition[20] = new Point(UALX, UALY, UALZ, EPISODE_NR, TIME, "UAL", action);
00070                   newPosition[21] = new Point(HALX, HALY, HALZ, EPISODE_NR, TIME, "HAL", action);
00071                   newPosition[22] = new Point(FILX, FILY, FILZ, EPISODE_NR, TIME, "FIL", action);
00072                   newPosition[23] = new Point(SBRX, SBRY, SBRZ, EPISODE_NR, TIME, "SBR", action);
00073                   newPosition[24] = new Point(OARX, OARY, OARZ, EPISODE_NR, TIME, "OAR", action);
00074                   newPosition[25] = new Point(UARX, UARY, UARZ, EPISODE_NR, TIME, "UAR", action);
00075                   newPosition[26] = new Point(HARX, HARY, HARZ, EPISODE_NR, TIME, "HAR", action);                 
00076                   newPosition[27] = new Point(FIRX, FIRY, FIRZ, EPISODE_NR, TIME, "FIR", action);
00077                   
00078 //                // invert z
00079 //                for(int i = 0; i < newPosition.length; i++)
00080 //                        newPosition[i].z *= -1;
00081                   
00082                   jointPositions.add(newPosition);
00083                   defaultColor = color;
00084         }
00085         
00086         public ArrayList<Point[]> getSequence() {
00087                 return jointPositions;
00088         }
00089 
00090         public void addPose(float episode_nr, float time, String action, float[] coords) {
00091                 addPose(episode_nr, time, action, coords[0], coords[1], coords[2], coords[3], coords[4], coords[5], coords[6], coords[7], coords[8], coords[9], coords[10], coords[11], coords[12], coords[13], coords[14], coords[15], coords[16], coords[17], coords[18], coords[19], coords[20], coords[21], coords[22], coords[23], coords[24], coords[25], coords[26], coords[27], coords[28], coords[29], coords[30], coords[31], coords[32], coords[33], coords[34], coords[35], coords[36], coords[37], coords[38], coords[39], coords[40], coords[41], coords[42], coords[43], coords[44], coords[45], coords[46], coords[47], coords[48], coords[49], coords[50], coords[51], coords[52], coords[53], coords[54], coords[55], coords[56], coords[57], coords[58], coords[59], coords[60], coords[61], coords[62], coords[63], coords[64], coords[65], coords[66], coords[67], coords[68], coords[69], coords[70], coords[71], coords[72], coords[73], coords[74], coords[75], coords[76], coords[77], coords[78], coords[79], coords[80], coords[81], coords[82], coords[83]);
00092         }
00093         
00094         public void setColor(int color) {
00095                 this.color = color;
00096         }
00097         
00098         public void draw(Canvas c, int step) {          
00099                 drawPose(c, step);
00100                 
00101                 if(trackedBodyPart != null)
00102                         drawBodyPartTrajectory(c, trackedBodyPart, trackedBodyPartStartTime, step);
00103         }       
00104         
00105         public void drawPose(Canvas c, int step) {
00106                 if(wireMode)
00107                         drawPoseWire(c, step);
00108                 else
00109                         drawPoseFat(c, step);
00110         }
00111         
00112         public void drawPoseWire(Canvas c, int step) {
00113                 if(step > getMaxStep())
00114                         return;
00115                 
00116                 Point[] pose = this.jointPositions.get(step);
00117 
00118             //c.strokeWeight(5f);
00119             c.stroke(this.color);
00120                 
00121                 c.pushMatrix();
00122                 c.scale(100);
00123 
00124                 // draw trajectory
00125                 for(int i = 0; i < 9; i++) 
00126                         c.line(pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z);           
00127 
00128                 // draw head
00129                 c.pushMatrix();
00130                 c.translate(pose[8].x, pose[8].y, pose[8].z);
00131                 c.sphere(0.09f);
00132                 c.popMatrix();
00133 
00134                 // draw body segments
00135                 c.line(pose[0].x, pose[0].y, pose[0].z, pose[10].x, pose[10].y, pose[10].z);
00136                 c.line(pose[0].x, pose[0].y, pose[0].z, pose[14].x, pose[14].y, pose[14].z);
00137                 c.line(pose[6].x, pose[6].y, pose[6].z, pose[18].x, pose[18].y, pose[18].z);
00138                 c.line(pose[6].x, pose[6].y, pose[6].z, pose[23].x, pose[23].y, pose[23].z);
00139 
00140                 for(int i = 10; i < 13; i++) 
00141                         c.line(pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z);           
00142                 for(int i = 14; i < 17; i++) 
00143                         c.line(pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z);           
00144                 for(int i = 18; i < 22; i++) 
00145                         c.line(pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z);           
00146                 for(int i = 23; i < 27; i++) 
00147                         c.line(pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z);           
00148                 
00149                 c.popMatrix();
00150         }
00151         
00152         public void drawPoseFat(Canvas c, int step) {
00153                 if(step > getMaxStep())
00154                         return;
00155                         
00156                 Point[] pose = this.jointPositions.get(step);
00157 
00158           //  c.strokeWeight(5f);
00159             c.color(color);
00160             c.fill(color);
00161                 
00162                 c.pushMatrix();
00163                 c.scale(100);
00164                 c.sphereDetail(20);
00165 
00166                 // draw trajectory
00167                 for(int i = 0; i < 8; i++){ 
00168                         c.color(0xffff0000);
00169                         cyl(c,pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z, spineThickness);
00170                         new Sphere(pose[i].x, pose[i].y, pose[i].z, -spineThickness, this.color).draw(c);
00171                 }
00172                 
00173                 // draw head
00174                 new Sphere(pose[8].x, pose[8].y, pose[8].z, -spineThickness*2, color).draw(c);  
00175                 // draw cone for gaze direction
00176                 float lambda = 1.1f;
00177                 cone(c,pose[8].x, pose[8].y, pose[8].z, pose[8].x + lambda*(pose[9].x - pose[8].x), pose[8].y + lambda*(pose[9].y - pose[8].y), pose[8].z+ + lambda*(pose[9].z - pose[8].z), spineThickness, spineThickness * 0.05f);           
00178 
00179                 // draw body segments
00180                 cyl(c,pose[0].x, pose[0].y, pose[0].z, pose[10].x, pose[10].y, pose[10].z, armsThickness);
00181                 cyl(c,pose[0].x, pose[0].y, pose[0].z, pose[14].x, pose[14].y, pose[14].z, armsThickness);
00182                 cyl(c,pose[6].x, pose[6].y, pose[6].z, pose[18].x, pose[18].y, pose[18].z, armsThickness);
00183                 cyl(c,pose[6].x, pose[6].y, pose[6].z, pose[23].x, pose[23].y, pose[23].z, armsThickness);
00184                 
00185                 for(int i = 10; i < 13; i++) {
00186                         cyl(c,pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z, armsThickness);
00187                         new Sphere(pose[i].x, pose[i].y, pose[i].z, -armsThickness, this.color).draw(c);
00188                 }
00189                 for(int i = 14; i < 17; i++){
00190                         cyl(c,pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z, armsThickness);
00191                         new Sphere(pose[i].x, pose[i].y, pose[i].z, -armsThickness, this.color).draw(c);
00192                 }
00193                 for(int i = 18; i < 21; i++){ 
00194                         cyl(c,pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z, armsThickness);
00195                         new Sphere(pose[i].x, pose[i].y, pose[i].z, -armsThickness, this.color).draw(c);
00196                 }
00197                 
00198                 cone(c,pose[21].x, pose[21].y, pose[21].z, pose[21].x + lambda*(pose[22].x - pose[21].x), pose[21].y + lambda*(pose[22].y - pose[21].y), pose[21].z+ + lambda*(pose[22].z - pose[21].z), armsThickness, armsThickness * 0.1f);
00199                 
00200                 for(int i = 23; i < 26; i++){ 
00201                         cyl(c,pose[i].x, pose[i].y, pose[i].z, pose[i + 1].x, pose[i + 1].y, pose[i + 1].z, armsThickness);
00202                         drawSphere(c, pose[i].x, pose[i].y, pose[i].z, armsThickness);
00203                 }
00204                 
00205                 cone(c,pose[26].x, pose[26].y, pose[26].z, pose[26].x + lambda*(pose[27].x - pose[26].x), pose[26].y + lambda*(pose[27].y - pose[26].y), pose[26].z+ + lambda*(pose[27].z - pose[26].z), armsThickness, armsThickness * 0.1f);
00206                 
00207                 // elbows
00208                 drawSphere(c, pose[20].x, pose[20].y, pose[20].z, armsThickness);
00209                 drawSphere(c, pose[25].x, pose[25].y, pose[25].z, armsThickness);
00210                 
00211                 c.popMatrix();
00212                 
00213         }
00214         
00215         protected void drawSphere(Canvas c, float x, float y, float z, float r) {
00216                 new Sphere(x, y, z, -r, this.color).draw(c);
00217         }
00218         
00219         protected void cyl(Canvas c, float x1, float y1, float z1, float x2, float y2, float z2, float r){
00220                 ConePrimitive cyl = new ConePrimitive(new Vector3f(x1, y1, z1), new Vector3f(x2, y2, z2), r);
00221                 cyl.draw(c);
00222         }
00223         
00224         
00225         protected void cone(Canvas c, float x1, float y1, float z1, float x2, float y2, float z2, float r1, float r2){
00226                 ConePrimitive cyl = new ConePrimitive(new Vector3f(x1, y1, z1), new Vector3f(x2, y2, z2), r1, r2);
00227                 cyl.draw(c);
00228         }
00229         
00230         public void trackBodyPart(Integer bodyPart, int startTime) {
00231                 this.trackedBodyPart = bodyPart;
00232                 this.trackedBodyPartStartTime = startTime;
00233         }
00234         
00235         public boolean isTrackingBodyPart() {
00236                 return trackedBodyPart != null;
00237         }
00238 
00239         public void drawBodyPartTrajectory(Canvas c, int bodyPart, int begin, int end) {                
00240                 c.pushMatrix();
00241                 c.stroke(255,255,0);
00242                 c.scale(100);
00243                 if((bodyPart != -1) && (end > 0)) {
00244                         for(int i = begin; i < end - 1; i++) {
00245                                 Point[] pose1 = jointPositions.get(i); 
00246                                 Point[] pose2 = jointPositions.get(i+1);
00247                                 c.line(pose1[bodyPart].x, pose1[bodyPart].y, pose1[bodyPart].z, pose2[bodyPart].x, pose2[bodyPart].y, pose2[bodyPart].z);
00248                         }
00249                 }
00250                 c.popMatrix();
00251         }         
00252         
00253         public int getMaxStep() {
00254                 return jointPositions.size()-1;
00255         }
00256         
00257         public boolean isEmpty() {
00258                 return jointPositions.size() == 0;
00259         }
00260         
00267     public void readData(File matlabAsciiFile) throws NumberFormatException, IOException {
00268                 BufferedReader r = new BufferedReader(new FileReader(matlabAsciiFile));
00269                 String line;
00270                 while((line = r.readLine()) != null) {
00271                         String[] parts = line.trim().split("\\s+");
00272                         float[] coords = new float[parts.length];
00273                         for(int i = 0; i < parts.length; i++)
00274                                 coords[i] = Float.parseFloat(parts[i]);
00275                         this.addPose(1, 0, "", coords);
00276                 }
00277     }
00278 
00279     public void setMode(boolean wireOnly) {
00280         wireMode = wireOnly;
00281     }
00282 
00283         @Override
00284         public void setColor(int color, int start, int end) {
00285                 System.err.println("not availible");
00286                 
00287         }
00288 }


mod_vis
Author(s): Moritz Tenorth, Jakob Engel
autogenerated on Mon Oct 6 2014 01:30:00