robot::robot(std::string filename,bool hideCollisionLinks,bool hideJoints,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool showConvexDecompositionDlg,bool centerAboveGround,bool makeModel,bool noSelfCollision,bool positionCtrl): filenameAndPath(filename)
{
printToConsole("URDF import operation started.");
openFile();
readJoints();
readLinks();
readSensors();
createJoints(hideJoints,positionCtrl);
createLinks(hideCollisionLinks,convexDecomposeNonConvexCollidables,createVisualIfNone,showConvexDecompositionDlg);
createSensors();
std::vector<int> parentlessObjects;
std::vector<int> allShapes;
std::vector<int> allObjects;
std::vector<int> allSensors;
for (int i=0;i<int(vLinks.size());i++)
{
if (simGetObjectParent(vLinks[i]->nLinkVisual)==-1)
parentlessObjects.push_back(vLinks[i]->nLinkVisual);
allObjects.push_back(vLinks[i]->nLinkVisual);
allShapes.push_back(vLinks[i]->nLinkVisual);
if (vLinks[i]->nLinkCollision!=-1)
{
if (simGetObjectParent(vLinks[i]->nLinkCollision)==-1)
parentlessObjects.push_back(vLinks[i]->nLinkCollision);
allObjects.push_back(vLinks[i]->nLinkCollision);
allShapes.push_back(vLinks[i]->nLinkCollision);
}
}
for (int i=0;i<int(vJoints.size());i++)
{
if (vJoints[i]->nJoint!=-1)
{
if (simGetObjectParent(vJoints[i]->nJoint)==-1)
parentlessObjects.push_back(vJoints[i]->nJoint);
allObjects.push_back(vJoints[i]->nJoint);
}
}
for (int i=0;i<int(vSensors.size());i++)
{
if (vSensors[i]->nSensor!=-1)
{
if (simGetObjectParent(vSensors[i]->nSensor)==-1)
parentlessObjects.push_back(vSensors[i]->nSensor);
allObjects.push_back(vSensors[i]->nSensor);
allSensors.push_back(vSensors[i]->nSensor);
}
if (vSensors[i]->nSensorAux!=-1)
{
allObjects.push_back(vSensors[i]->nSensorAux);
allSensors.push_back(vSensors[i]->nSensorAux);
}
}
// If we want to alternate respondable mask:
if (!noSelfCollision)
{
for (int i=0;i<int(parentlessObjects.size());i++)
setLocalRespondableMaskCummulative_alternate(parentlessObjects[i],true);
}
// Now center the model:
if (centerAboveGround)
{
bool firstValSet=false;
C3Vector minV,maxV;
for (int shNb=0;shNb<int(allShapes.size());shNb++)
{
float* vertices;
int verticesSize;
int* indices;
int indicesSize;
if (simGetShapeMesh(allShapes[shNb],&vertices,&verticesSize,&indices,&indicesSize,NULL)!=-1)
{
C7Vector tr;
simGetObjectPosition(allShapes[shNb],-1,tr.X.data);
C3Vector euler;
simGetObjectOrientation(allShapes[shNb],-1,euler.data);
tr.Q.setEulerAngles(euler);
for (int i=0;i<verticesSize/3;i++)
{
C3Vector v(vertices+3*i);
v*=tr;
if (!firstValSet)
{
minV=v;
maxV=v;
firstValSet=true;
}
else
{
minV.keepMin(v);
maxV.keepMax(v);
}
}
simReleaseBuffer((char*)vertices);
simReleaseBuffer((char*)indices);
}
}
C3Vector shiftAmount((minV+maxV)*-0.5f);
shiftAmount(2)+=(maxV(2)-minV(2))*0.5f;
for (int i=0;i<int(parentlessObjects.size());i++)
{
C3Vector p;
simGetObjectPosition(parentlessObjects[i],-1,p.data);
p+=shiftAmount;
simSetObjectPosition(parentlessObjects[i],-1,p.data);
}
}
// Now create a model bounding box if that makes sense:
if ((makeModel)&&(parentlessObjects.size()==1))
{
int p=simGetModelProperty(parentlessObjects[0]);
p|=sim_modelproperty_not_model;
simSetModelProperty(parentlessObjects[0],p-sim_modelproperty_not_model);
for (int i=0;i<int(allObjects.size());i++)
{
if (allObjects[i]!=parentlessObjects[0])
{
int p=simGetObjectProperty(allObjects[i]);
simSetObjectProperty(allObjects[i],p|sim_objectproperty_selectmodelbaseinstead);
}
}
for (int i=0;i<int(allSensors.size());i++)
{
if (allSensors[i]!=parentlessObjects[0])
{
int p=simGetObjectProperty(allSensors[i]);
simSetObjectProperty(allSensors[i],p|sim_objectproperty_dontshowasinsidemodel); // sensors are usually large and it is ok if they do not appear as inside of the model bounding box!
}
}
}
// Now select all new objects:
simRemoveObjectFromSelection(sim_handle_all,-1);
for (int i=0;i<int(allObjects.size());i++)
simAddObjectToSelection(sim_handle_single,allObjects[i]);
printToConsole("URDF import operation finished.\n\n");
}
void Sensors::onCreateArea()
{
createSensors();
}
