[Major] Now handle prismatic joint when parsing urdf. Only cartesion X,Y or Z...

[Major] Now handle prismatic joint when parsing urdf. Only cartesion X,Y or Z are accepted, unaligned axis bring crash

src/multibody/parser/urdf.hpp

@@ -123,12 +123,38 @@ namespace se3
 		  }
 		break;
 	      }
+	    case ::urdf::Joint::PRISMATIC:
+	      {
+	    AxisCartesian axis = extractCartesianAxis(joint->axis);  	
+		switch(axis)
+		  {
+		  case AXIS_X:
+		    model.addBody( parent, JointModelPX(), jointPlacement, Y, joint->name,link->name, visual );
+		    break;
+		  case AXIS_Y:
+		    model.addBody( parent, JointModelPY(), jointPlacement, Y, joint->name,link->name, visual );
+		    break;
+		  case AXIS_Z:
+		    model.addBody( parent, JointModelPZ(), jointPlacement, Y, joint->name,link->name, visual );
+		    break;
+		  case AXIS_UNALIGNED:
+		  	std::cerr << "Bad axis = (" <<joint->axis.x<<","<<joint->axis.y
+			      <<","<<joint->axis.z<<")" << std::endl;
+		    assert(false && "Only X, Y or Z axis are accepted." );
+		  	break;
+		  default:
+		    assert( false && "Fatal Error while extracting revolute joint axis");
+		    break;
+		  }
+		break;
+	      }
 	    case ::urdf::Joint::FIXED:
 	      {
 		/* To fixed this, "spot" point should be added. TODO. */
 		//std::cerr << "For now, fixed joint are not accepted. " << std::endl;
 		break;
 	      }
+	    
 	    default:
 	      {
 		std::cerr << "The joint type " << joint->type << " is not supported." << std::endl;