int urdfLink::scaleShapeIfRequired(int shapeHandle,float scalingFactors[3])
{ // in future there will be a non-iso scaling function for objects in V-REP, but until then...
if ( (scalingFactors[0]*scalingFactors[1]*scalingFactors[2]>0.99999f)&&(scalingFactors[0]>0.0f)&&(scalingFactors[1]>0.0f) )
return(shapeHandle); // no scaling required!
if (fabs(scalingFactors[0])<0.00001f)
scalingFactors[0]=0.00001f*scalingFactors[0]/fabs(scalingFactors[0]);
if (fabs(scalingFactors[1])<0.00001f)
scalingFactors[1]=0.00001f*scalingFactors[1]/fabs(scalingFactors[1]);
if (fabs(scalingFactors[2])<0.00001f)
scalingFactors[2]=0.00001f*scalingFactors[2]/fabs(scalingFactors[2]);
int newShapeHandle=shapeHandle;
float* vertices;
int verticesSize;
int* indices;
int indicesSize;
if (simGetShapeMesh(shapeHandle,&vertices,&verticesSize,&indices,&indicesSize,NULL)!=-1)
{
// Scale the vertices:
C7Vector tr;
simGetObjectPosition(shapeHandle,-1,tr.X.data);
C3Vector euler;
simGetObjectOrientation(shapeHandle,-1,euler.data);
tr.Q.setEulerAngles(euler);
for (int i=0;i<verticesSize/3;i++)
{
C3Vector v(vertices+3*i);
v*=tr;
v(0)*=scalingFactors[0];
v(1)*=scalingFactors[1];
v(2)*=scalingFactors[2];
vertices[3*i+0]=v(0);
vertices[3*i+1]=v(1);
vertices[3*i+2]=v(2);
}
// Flip the triangles (if needed)
if (scalingFactors[0]*scalingFactors[1]*scalingFactors[2]<0.0f)
{
for (int i=0;i<indicesSize/3;i++)
{
int tmp=indices[3*i+0];
indices[3*i+0]=indices[3*i+1];
indices[3*i+1]=tmp;
}
}
// Remove the old shape and create a new one with the scaled data:
simRemoveObject(shapeHandle);
newShapeHandle=simCreateMeshShape(2,20.0f*piValue/180.0f,vertices,verticesSize,indices,indicesSize,NULL);
simReleaseBuffer((char*)vertices);
simReleaseBuffer((char*)indices);
}
return(newShapeHandle);
}
//
// Get cue position and orientation
//
bool get_cue_position_orientation()
{
bool success = simGetObjectOrientation(tip_handle, WORLD_FRAME, tip_orientation);
success &= simGetObjectPosition(tip_handle, WORLD_FRAME, tip_position);
table_state.cue_position.x= tip_position[0];
table_state.cue_position.y = tip_position[1];
table_state.cue_position.z = tip_position[2];
table_state.cue_orientation[0] = tip_orientation[0];
table_state.cue_orientation[1] = tip_orientation[1];
table_state.cue_orientation[2] = tip_orientation[2];
return success;
}
static simInt groupShapes(std::vector<urdfElement> &elements)
{
simInt n = -1;
simInt *shapes = new simInt[elements.size()];
int validShapes = 0;
for (unsigned int i=0; i<elements.size(); i++) {
urdfElement &element = elements[i];
if (element.n!=-1) {
simSetShapeColor(element.n,NULL,0,element.rgba);
C7Vector frame;
frame.X.set(element.xyz);
frame.Q=getQuaternionFromRpy(element.rpy);
C7Vector initFrame;
simGetObjectPosition(element.n,-1,initFrame.X.data);
C3Vector euler;
simGetObjectOrientation(element.n,-1,euler.data);
initFrame.Q.setEulerAngles(euler);
C7Vector x(frame*initFrame);
//C7Vector x(frame);
simSetObjectPosition(element.n,-1,x.X.data);
simSetObjectOrientation(element.n,-1,x.Q.getEulerAngles().data);
shapes[validShapes++] = element.n;
}
}
if (validShapes > 1) {
n = simGroupShapes(shapes, validShapes);
} else if (validShapes == 1) {
n = shapes[0];
}
delete shapes;
return n;
}
//Write
void urdfLink::createLink(bool hideCollisionLinks,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool& showConvexDecompositionDlg)
{
std::string txt("Creating link '"+name+"'...");
printToConsole(txt.c_str());
//visuals.clear();
// Visuals
for (int i=0; i<visuals.size(); i++) {
urdfElement &visual = visuals[i];
if(!visual.meshFilename.empty())
{
std::string fname(visual.meshFilename);
bool exists=true;
bool useAlt=false;
if (!simDoesFileExist(fname.c_str()))
{
fname=visual.meshFilename_alt;
exists=simDoesFileExist(fname.c_str());
useAlt=true;
}
if (!exists)
printToConsole("ERROR: the mesh file could not be found.");
else
visual.n = simImportShape(visual.meshExtension,fname.c_str(),0,0.0001f,1.0);
if (!visual.n)
{
if (!useAlt)
txt="ERROR: failed to create the mesh '"+visual.meshFilename+"' with extension type "+boost::lexical_cast<std::string>(visual.meshExtension);
else
txt="ERROR: failed to create the mesh '"+visual.meshFilename+"' or '"+visual.meshFilename_alt+"' with extension type "+boost::lexical_cast<std::string>(visual.meshExtension);
printToConsole(txt.c_str());
}
else
visual.n = scaleShapeIfRequired(visual.n,visual.mesh_scaling);
}
else if (!isArrayEmpty(visual.sphere_size))
visual.n = simCreatePureShape( 1,1+2+16, visual.sphere_size, mass, NULL);
else if (!isArrayEmpty(visual.cylinder_size))
visual.n = simCreatePureShape( 2,1+2+16, visual.cylinder_size, mass, NULL);
else if (!isArrayEmpty(visual.box_size))
visual.n = simCreatePureShape( 0,1+2+16, visual.box_size, mass, NULL);
}
//collisions.clear();
//mass=0.1;
//collision
for (int i=0; i<collisions.size(); i++) {
urdfElement &collision = collisions[i];
if(!collision.meshFilename.empty())
{
std::string fname(collision.meshFilename);
bool exists=true;
bool useAlt=false;
if (!simDoesFileExist(fname.c_str()))
{
fname=collision.meshFilename_alt;
exists=simDoesFileExist(fname.c_str());
useAlt=true;
}
if (!exists)
printToConsole("ERROR: the mesh file could not be found");
else
collision.n = simImportShape(collision.meshExtension,fname.c_str(),0,0.0001f,1.0);
if (collision.n == -1)
{
if (!useAlt)
txt="ERROR: failed to create the mesh '"+collision.meshFilename+"' with extension type "+boost::lexical_cast<std::string>(collision.meshExtension);
else
txt="ERROR: failed to create the mesh '"+collision.meshFilename+"' or '"+collision.meshFilename_alt+"' with extension type "+boost::lexical_cast<std::string>(collision.meshExtension);
printToConsole(txt.c_str());
}
else
{
collision.n=scaleShapeIfRequired(collision.n,collision.mesh_scaling);
if (createVisualIfNone&&(visuals.size()==0))
{ // We create a visual from the collision shape (before it gets morphed hereafter):
simRemoveObjectFromSelection(sim_handle_all,-1);
simAddObjectToSelection(sim_handle_single,collision.n);
simCopyPasteSelectedObjects();
addVisual();
currentVisual().n = simGetObjectLastSelection();
}
int p;
int convInts[5]={1,500,200,0,0}; // 3rd value from 100 to 500 on 5/2/2014
float convFloats[5]={100.0f,30.0f,0.25f,0.0f,0.0f};
if ( convexDecomposeNonConvexCollidables&&(simGetObjectIntParameter(collision.n,3017,&p)>0)&&(p==0) )
{
int aux=1+4+8+16+64;
if (showConvexDecompositionDlg)
aux=1+2+8+16+64;
showConvexDecompositionDlg=false;
simConvexDecompose(collision.n,aux,convInts,convFloats); // we generate convex shapes!
}
simSetObjectIntParameter(collision.n,3003,!inertiaPresent); // we make it non-static if there is an inertia
simSetObjectIntParameter(collision.n,3004,1); // we make it respondable since it is a collision object
}
}
else if (!isArrayEmpty(collision.sphere_size))
collision.n = simCreatePureShape( 1,1+2+4+8+16*(!inertiaPresent), collision.sphere_size, mass, NULL);
else if (!isArrayEmpty(collision.cylinder_size))
collision.n = simCreatePureShape( 2,1+2+4+8+16*(!inertiaPresent), collision.cylinder_size, mass, NULL);
else if (!isArrayEmpty(collision.box_size))
collision.n = simCreatePureShape( 0,1+2+4+8+16*(!inertiaPresent), collision.box_size, mass, NULL);
}
// Hack to draw COM in the collision layer
/*
addCollision();
currentCollision().xyz[0] = inertial_xyz[0];
currentCollision().xyz[1] = inertial_xyz[1];
currentCollision().xyz[0] = inertial_xyz[2];
currentCollision().rpy[0] = 1.5;
float dummySize[3]={0.01f,0.01f,0.01f};
currentCollision().n = simCreatePureShape( 1,1+2+16, dummySize, mass, NULL);
*/
// Grouping collisions shapes
nLinkCollision = groupShapes(collisions);
// Inertia
if (inertiaPresent)
{
C3Vector euler;
if (nLinkCollision==-1)
{ // we do not have a collision object. Let's create a dummy collision object, since inertias can't exist on their own in V-REP:
float dummySize[3]={0.01f,0.01f,0.01f};
//nLinkCollision = simCreatePureShape( 1,1+2+4, dummySize, mass, NULL); // we make it non-respondable!
nLinkCollision = simCreatePureShape( 1,1+2+16, dummySize, mass, NULL);
}
C7Vector inertiaFrame;
inertiaFrame.X.set(inertial_xyz);
inertiaFrame.Q=getQuaternionFromRpy(inertial_rpy);
//simSetObjectPosition(nLinkCollision,-1,inertiaFrame.X.data);
//C7Vector collisionFrame;
//collisionFrame.X.set(collision_xyz);
//collisionFrame.Q=getQuaternionFromRpy(collision_rpy);
C7Vector collisionFrame;
simGetObjectPosition(nLinkCollision,-1,collisionFrame.X.data);
simGetObjectOrientation(nLinkCollision,-1,euler.data);
collisionFrame.Q.setEulerAngles(euler);
//C4X4Matrix x((collisionFrame.getInverse()*inertiaFrame).getMatrix());
C4X4Matrix x(inertiaFrame.getMatrix());
float i[12]={x.M(0,0),x.M(0,1),x.M(0,2),x.X(0),x.M(1,0),x.M(1,1),x.M(1,2),x.X(1),x.M(2,0),x.M(2,1),x.M(2,2),x.X(2)};
simSetShapeMassAndInertia(nLinkCollision,mass,inertia,C3Vector::zeroVector.data,i);
//std::cout << "Mass: " << mass << std::endl;
}
else
{
if (nLinkCollision!=-1)
{
std::string txt("ERROR: found a collision object without inertia data for link '"+ name+"'. Is that link meant to be static?");
printToConsole(txt.c_str());
}
}
if (createVisualIfNone&&(visuals.size()==0)&&(nLinkCollision!=-1))
{ // We create a visual from the collision shape (meshes were handled earlier):
addVisual();
urdfElement &visual = currentVisual();
simRemoveObjectFromSelection(sim_handle_all,-1);
simAddObjectToSelection(sim_handle_single,nLinkCollision);
simCopyPasteSelectedObjects();
visual.n=simGetObjectLastSelection();
simSetObjectIntParameter(visual.n,3003,1); // we make it static since only visual
simSetObjectIntParameter(visual.n,3004,0); // we make it non-respondable since only visual
}
// Set the respondable mask:
if (nLinkCollision!=-1)
simSetObjectIntParameter(nLinkCollision,3019,0xff00); // colliding with everything except with other objects in that tree hierarchy
// Grouping shapes
nLinkVisual = groupShapes(visuals);
// Set the names, visibility, etc.:
if (nLinkVisual!=-1)
{
setVrepObjectName(nLinkVisual,std::string(name+"_visual").c_str());
const float specularDiffuse[3]={0.3f,0.3f,0.3f};
if (nLinkCollision!=-1)
{ // if we have a collision object, we attach the visual object to it, then forget the visual object
C7Vector collisionFrame;
C3Vector euler;
simGetObjectPosition(nLinkCollision,-1,collisionFrame.X.data);
simGetObjectOrientation(nLinkCollision,-1,euler.data);
collisionFrame.Q.setEulerAngles(euler);
C7Vector visualFrame;
simGetObjectPosition(nLinkVisual,-1,visualFrame.X.data);
simGetObjectOrientation(nLinkVisual,-1,euler.data);
visualFrame.Q.setEulerAngles(euler);
C7Vector x(collisionFrame.getInverse()*visualFrame);
simSetObjectPosition(nLinkVisual,-1,x.X.data);
simSetObjectOrientation(nLinkVisual,-1,x.Q.getEulerAngles().data);
simSetObjectParent(nLinkVisual,nLinkCollision,0);
}
}
if (nLinkCollision!=-1)
{
setVrepObjectName(nLinkCollision,std::string(name+"_respondable").c_str());
if (hideCollisionLinks)
simSetObjectIntParameter(nLinkCollision,10,256); // we "hide" that object in layer 9
}
}
void robot::createLinks(bool hideCollisionLinks,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool showConvexDecompositionDlg)
{
std::string txt("There are "+boost::lexical_cast<std::string>(vLinks.size())+" links.");
printToConsole(txt.c_str());
for(size_t i = 0; i < vLinks.size() ; i++)
{
urdfLink *Link = vLinks.at(i);
Link->createLink(hideCollisionLinks,convexDecomposeNonConvexCollidables,createVisualIfNone,showConvexDecompositionDlg);
// We attach the collision link to a joint. If the collision link isn't there, we use the visual link instead:
C7Vector linkInitialConf;
C7Vector linkDesiredConf;
int effectiveLinkHandle=-1;
if (Link->nLinkCollision!=-1)
{
effectiveLinkHandle=Link->nLinkCollision;
if (effectiveLinkHandle != -1) {
// Visual object position and orientation is already set in the Link
float xyz[3] = {0,0,0};
float rpy[3] = {0,0,0};
linkDesiredConf.X.set(xyz);
linkDesiredConf.Q=getQuaternionFromRpy(rpy);
}
}
else
{
effectiveLinkHandle = Link->nLinkVisual;
if (effectiveLinkHandle != -1) {
// Visual object position and orientation is already set in the Link
float xyz[3] = {0,0,0};
float rpy[3] = {0,0,0};
linkDesiredConf.X.set(xyz);
linkDesiredConf.Q=getQuaternionFromRpy(rpy);
}
}
simGetObjectPosition(effectiveLinkHandle,-1,linkInitialConf.X.data);
C3Vector euler;
simGetObjectOrientation(effectiveLinkHandle,-1,euler.data);
linkInitialConf.Q.setEulerAngles(euler);
// C7Vector trAbs(linkDesiredConf*linkInitialConf); // still local
C7Vector trAbs(linkInitialConf);
int parentJointIndex=getJointPosition(Link->parent);
if( parentJointIndex!= -1)
{
joint* Joint = vJoints.at(parentJointIndex);
trAbs=Joint->jointBaseFrame*trAbs; // now absolute
}
//set the transfrom matrix to the object
simSetObjectPosition(effectiveLinkHandle,-1,trAbs.X.data);
simSetObjectOrientation(effectiveLinkHandle,-1,trAbs.Q.getEulerAngles().data);
}
// Finally the real parenting:
for(size_t i = 0; i < vJoints.size() ; i++)
{
int parentLinkIndex=getLinkPosition(vJoints.at(i)->parentLink);
if (parentLinkIndex!=-1)
{
urdfLink* parentLink=vLinks[parentLinkIndex];
if (parentLink->nLinkCollision!=-1)
simSetObjectParent(vJoints.at(i)->nJoint,parentLink->nLinkCollision,true);
else
simSetObjectParent(vJoints.at(i)->nJoint,parentLink->nLinkVisual,true);
}
int childLinkIndex=getLinkPosition(vJoints.at(i)->childLink);
if (childLinkIndex!=-1)
{
urdfLink* childLink=vLinks[childLinkIndex];
if (childLink->nLinkCollision!=-1)
simSetObjectParent(childLink->nLinkCollision,vJoints.at(i)->nJoint,true);
else
simSetObjectParent(childLink->nLinkVisual,vJoints.at(i)->nJoint,true);
}
}
}
void robot::createJoints(bool hideJoints,bool positionCtrl)
{
std::string txt("There are "+boost::lexical_cast<std::string>(vJoints.size())+" joints.");
printToConsole(txt.c_str());
//Set parents and childs for all the links
for(size_t i = 0; i < vJoints.size() ; i++)
{
vLinks.at(getLinkPosition(vJoints.at(i)->parentLink))->child = vJoints.at(i)->name;
vLinks.at(getLinkPosition(vJoints.at(i)->childLink))->parent = vJoints.at(i)->name;
}
//Create the joints
for(size_t i = 0; i < vJoints.size() ; i++)
{
//Move the joints to the positions specifieds by the urdf file
C7Vector tmp;
tmp.setIdentity();
tmp.X.set(vJoints.at(i)->origin_xyz);
tmp.Q=getQuaternionFromRpy(vJoints.at(i)->origin_rpy);
vJoints.at(i)->jointBaseFrame=vJoints.at(i)->jointBaseFrame*tmp;
//Set name jointParent to each joint
int nParentLink = getLinkPosition(vJoints.at(i)->parentLink);
vJoints.at(i)->parentJoint = vLinks.at(nParentLink)->parent;
//Create the joint/forceSensor/dummy:
if (vJoints.at(i)->jointType==-1)
vJoints.at(i)->nJoint = simCreateDummy(0.02f,NULL); // when joint type was not recognized
if (vJoints.at(i)->jointType==0)
vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
if (vJoints.at(i)->jointType==1)
vJoints.at(i)->nJoint = simCreateJoint(sim_joint_prismatic_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
if (vJoints.at(i)->jointType==2)
vJoints.at(i)->nJoint = simCreateJoint(sim_joint_spherical_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
if (vJoints.at(i)->jointType==3)
vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
if (vJoints.at(i)->jointType==4)
{ // when joint type is "fixed"
int intParams[5]={1,4,4,0,0};
float floatParams[5]={0.02f,1.0f,1.0f,0.0f,0.0f};
vJoints.at(i)->nJoint = simCreateForceSensor(0,intParams,floatParams,NULL);
}
if ( (vJoints.at(i)->jointType==0)||(vJoints.at(i)->jointType==1) )
{
float interval[2]={vJoints.at(i)->lowerLimit,vJoints.at(i)->upperLimit-vJoints.at(i)->lowerLimit};
simSetJointInterval(vJoints.at(i)->nJoint,0,interval);
if (vJoints.at(i)->jointType==0)
{ // revolute
simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitAngular);
simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitAngular);
}
else
{ // prismatic
simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitLinear);
simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitLinear);
}
// We turn the position control on:
if (positionCtrl)
{
simSetObjectIntParameter(vJoints.at(i)->nJoint,2000,1);
simSetObjectIntParameter(vJoints.at(i)->nJoint,2001,1);
}
}
//Set the name:
setVrepObjectName(vJoints.at(i)->nJoint,vJoints.at(i)->name.c_str());
if (hideJoints)
simSetObjectIntParameter(vJoints.at(i)->nJoint,10,512); // layer 10
}
//Set positions to joints from the 4x4matrix
for(size_t i = 0; i < vJoints.size() ; i++)
{
simSetObjectPosition(vJoints.at(i)->nJoint,-1,vJoints.at(i)->jointBaseFrame.X.data);
simSetObjectOrientation(vJoints.at(i)->nJoint,-1 ,vJoints.at(i)->jointBaseFrame.Q.getEulerAngles().data);
}
//Set joint parentship between them (thes parentship will be remove before adding the joints)
for(size_t i = 0; i < vJoints.size() ; i++)
{
int parentJointIndex=getJointPosition(vJoints.at(i)->parentJoint);
if ( parentJointIndex!= -1)
{
simInt nParentJoint = vJoints.at(parentJointIndex)->nJoint;
simInt nJoint = vJoints.at(i)->nJoint;
simSetObjectParent(nJoint,nParentJoint,false);
}
}
//Delete all the partnership without moving the joints but after doing that update the transform matrix
for(size_t i = 0; i < vJoints.size() ; i++)
{
C4X4Matrix tmp;
simGetObjectPosition(vJoints.at(i)->nJoint,-1,tmp.X.data);
C3Vector euler;
simGetObjectOrientation(vJoints.at(i)->nJoint,-1,euler.data);
tmp.M.setEulerAngles(euler);
vJoints.at(i)->jointBaseFrame = tmp;
simInt nJoint = vJoints.at(i)->nJoint;
simSetObjectParent(nJoint,-1,true);
}
for(size_t i = 0; i < vJoints.size() ; i++)
{
C4X4Matrix jointAxisMatrix;
jointAxisMatrix.setIdentity();
C3Vector axis(vJoints.at(i)->axis);
C3Vector rotAxis;
float rotAngle=0.0f;
if (axis(2)<1.0f)
{
if (axis(2)<=-1.0f)
rotAngle=3.14159265359f;
else
rotAngle=acosf(axis(2));
rotAxis(0)=-axis(1);
rotAxis(1)=axis(0);
rotAxis(2)=0.0f;
rotAxis.normalize();
C7Vector m(jointAxisMatrix);
float alpha=-atan2(rotAxis(1),rotAxis(0));
float beta=atan2(-sqrt(rotAxis(0)*rotAxis(0)+rotAxis(1)*rotAxis(1)),rotAxis(2));
C7Vector r;
r.X.clear();
r.Q.setEulerAngles(0.0f,0.0f,alpha);
m=r*m;
r.Q.setEulerAngles(0.0f,beta,0.0f);
m=r*m;
r.Q.setEulerAngles(0.0f,0.0f,rotAngle);
m=r*m;
r.Q.setEulerAngles(0.0f,-beta,0.0f);
m=r*m;
r.Q.setEulerAngles(0.0f,0.0f,-alpha);
m=r*m;
jointAxisMatrix=m.getMatrix();
}
C4Vector q((vJoints.at(i)->jointBaseFrame*jointAxisMatrix).Q);
simSetObjectOrientation(vJoints.at(i)->nJoint,-1,q.getEulerAngles().data);
}
}
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");
}
