void HelpMessageBox::showOrPrint()
{
#if defined(WIN32)
// On Windows, show a message box, as there is no stderr/stdout in windowed applications
exec();
#else
// On other operating systems, print help text to console
printToConsole();
#endif
}
int robot::getJointPosition(std::string jointName)
{
for(size_t i = 0; i < vJoints.size() ; i++)
{
if(vJoints.at(i)->name == jointName){return i;}
}
std::string txt("ERROR: there is no joint with name '"+ jointName+"'");
printToConsole(txt.c_str());
return -1;
}
int main(int argc, char **argv)
{
int i;
char buffer[1024];
futex_init(&printFutex, 1);
mythread_t mythread1[NTHREADS];
mythread_t signalingThread[NTHREADS/2];
mythread_setconcurrency(4);
mythread_mutex_init(&mutex,NULL);
mythread_cond_init(&condition,NULL);
mythread_barrier_init(&barrier,NULL,NTHREADS+1);
/* Create Threads */
for(i=0;i<NTHREADS;i++){
sprintf(buffer, "Created thread : %d\n", i+1);
printToConsole(buffer);
mythread_create(&mythread1[i],NULL,&thread_func, NULL);
}
/*Signal threads waiting on cond var*/
while(count<NTHREADS){
/* Special case for testing broadcast*/
if(count == NTHREADS/2){
mythread_cond_broadcast(&condition);
}else{
mythread_cond_signal(&condition);
}
}
/* Waiting on barrier. Last thread, or this main thread will unblock the barrier depending on the execution sequence */
mythread_barrier_wait(&barrier);
sprintf(buffer, "Out of barrier, main thread exiting..\n");
printToConsole(buffer);
/* Destroy mutex, barrier and cond*/
mythread_cond_destroy(&condition);
mythread_mutex_destroy(&mutex);
mythread_barrier_destroy(&barrier);
sprintf(buffer, "Finished Execution\n");
printToConsole(buffer);
}
//-- testfunction
void Spielfeld::explodeAll()
{
Position pos;
for(pos.y=0 ; pos.y < FELD_HEIGHT ; pos.y++){
for(pos.x=0 ; pos.x < FELD_WIDTH ; pos.x++){
if( getField(pos) == BOMBE )
{
explode(pos, ((Bombe*)feld[pos.x][pos.y])->getReichweite());
}
}
}
printToConsole();
}
void urdfLink::verifyInertia()
{
float c=0.0f;
for (int i=0;i<9;i++)
c+=fabs(inertia[i]);
if (c==0.0f)
{
std::string txt("ERROR: found an invalid inertia entry for link '"+ name+"'");
printToConsole(txt.c_str());
inertia[0]=0.001f;
inertia[4]=0.001f;
inertia[8]=0.001f;
}
}
ChessMove consolePlayer(ChessBoard &board, int color) {
string inputstr;
ChessMove chessMove;
printToConsole(board);
while (true) {
cout << '\n';
cin >> inputstr;
try {
chessMove=stringToMove(board,inputstr);
if (getColor(chessMove.piece)==color&&board.validateFromList(chessMove)) {
return chessMove;
} else cout << "Invalid move!\n";
} catch (invalid_argument &ex) {
cout << "Exception : " << ex.what() << '\n';
}
}
}
void urdfLink::setMeshFilename(std::string packagePath,std::string meshFilename,std::string choose)
{
std::cout << std::flush;
std::string meshFilename_alt; // we use an alternative filename... the package location is somewhat strangely defined sometimes!!
if (meshFilename.compare(0,10,"package://")==0) // condition added by Marc on 17/1/2014
{
meshFilename = meshFilename.substr(9,meshFilename.size()); //to delete de package:/ part
meshFilename_alt=meshFilename;
meshFilename = packagePath + meshFilename;
packagePath = packagePath.substr(0, packagePath.find_last_of("/"));
meshFilename_alt = packagePath + meshFilename_alt;
}
std::string extension = meshFilename.substr(meshFilename.size()-3,meshFilename.size());
int nExtension;
if(extension == "obj" || extension =="OBJ"){ nExtension = 0;}
else if(extension == "dxf" || extension == "DXF"){ nExtension = 1;}
else if(extension == "3ds" || extension == "3DS"){ nExtension = 2;}
else if(extension == "stl" || extension == "STL"){ nExtension = 4;}
else if(extension == "dae" || extension == "DAE"){ nExtension = 5;}
else
{
nExtension = -1;
std::string txt("ERROR: the extension '"+ extension +"' is not currently a supported.");
printToConsole(txt.c_str());
}
if(choose == "visual")
{
urdfElement &visual = currentVisual();
visual.meshFilename = meshFilename;
visual.meshFilename_alt = meshFilename_alt;
visual.meshExtension = nExtension;
}
if(choose == "collision")
{
urdfElement &collision = currentCollision();
collision.meshFilename = meshFilename;
collision.meshFilename_alt = meshFilename_alt;
collision.meshExtension = nExtension;
}
}
/* Each thread will increment the count (global) protected by mutex */
void* thread_func(void *arg)
{
char buffer[1024];
/*Lock Mutex*/
int temp = 0;
mythread_mutex_lock(&mutex);
while(temp<1000){
temp++;
}
/*Wait to increment until main thread signals*/
mythread_cond_wait(&condition,&mutex);
count++;
mythread_enter_kernel();
sprintf(buffer, "Incremented Count to : %d\n", count);
printToConsole(buffer);
mythread_leave_kernel();
mythread_mutex_unlock(&mutex);
/* Threads will wait on the barrier. Main thread will also wait */
mythread_barrier_wait(&barrier);
mythread_exit(NULL);
}
void Tracer::output (TraceLevel level, const char *msg, ...)
{
if (level < minLevel){
return;
}
{
va_list ap;
va_start (ap, msg);
int len = vsnprintf (NULL, 0, msg, ap);
va_end (ap);
char out[len+1];
va_start (ap, msg);
vsnprintf (out, len+1, msg, ap);
va_end (ap);
printToConsole(level, out);
}
}
void consoleGame() {
string inputstr;
ChessMove chessMove;
ChessBoard board;
board.setup();
for (int i=0;i<15;i++) {
cout << '\n';
cin >> inputstr;
if (inputstr=="q") break;
try {
chessMove=stringToMove(board,inputstr);
if (board.validateFromList(chessMove)) {
board.applyMove(chessMove);
printToConsole(board);
printBoardStats(board,true,true);
} else cout << "Invalid move!\n";
} catch (invalid_argument &ex) {
cout << "Exception : " << ex.what() << '\n';
}
}
}
void listCommands(){
textColor = LIGHTGRAY;
printToConsole("\tls \t\t\t\t\t\t- list all files\n");
printToConsole("\tcat [file] \t\t\t\t- view contents of that file\n");
printToConsole("\trm [file] \t\t\t\t- remove file with name\n");
printToConsole("\tchmod [mode] [file] \t- change file permissions\n");
printToConsole("\tte [file] \t\t\t\t- create or edit file with name\n");
printToConsole("\twc [file] \t\t\t\t- reports lines, words, and characters of a file\n");
printToConsole("\thistory \t\t\t\t- display previous 15 commands\n");
printToConsole("\tsu [user] \t\t\t\t- switch current user\n");
printToConsole("\tchown [user] [file]\t\t- change owner of a file\n");
printToConsole("\tadduser [user]\t\t\t- add new user\n");
printToConsole("\tdeluser [user]\t\t\t- remove user\n");
printToConsole("\tlistus \t\t\t\t\t- list users\n");
printToConsole("\techo [word]...\t\t\t- print text to screen\n");
printToConsole("\tpasswd\t\t\t\t\t- change password for current user\n");
printToConsole("\tpwd\t\t\t\t\t\t- print working directory\n");
printToConsole("\tcd [folder]\t\t\t\t- change directory to folder\n");
printToConsole("\tmkdir [directory]\t\t- make new directory\n");
printToConsole("\trmdir [directory]\t\t- remove empty directory\n");
printToConsole("\tmv [file] [directory]\t- move file to directory\n");
/*
printToConsole("\tcp [file] [new file]\t- make a copy of a file\n");
printToConsole("\tdiff [file 1] [file 2]\t- compare two files\n");
*/
}
/*!
* Print line with a prefix to the console
*/
void Logger::withPrefix(String logPrefix, String string) {
prefix = "[" + logPrefix + "]";
printToConsole(string);
}
void robot::createSensors()
{
std::string txt("There are "+boost::lexical_cast<std::string>(vSensors.size())+" sensors.");
printToConsole(txt.c_str());
for(size_t i = 0; i < vSensors.size() ; i++)
{
sensor *Sensor = vSensors.at(i);
if (Sensor->gazeboSpec)
printToConsole("ERROR: sensor will not be created: the URDF specification is supported, but this is a Gazebo tag which is not documented as it seems.");
else
{
if (Sensor->cameraSensorPresent)
{
int intParams[4]={Sensor->resolution[0],Sensor->resolution[1],0,0};
float floatParams[11]={Sensor->clippingPlanes[0],Sensor->clippingPlanes[1],60.0f*piValue/180.0f,0.2f,0.2f,0.4f,0.0f,0.0f,0.0f,0.0f,0.0f};
Sensor->nSensor=simCreateVisionSensor(1,intParams,floatParams,NULL);
//Set the name:
setVrepObjectName(Sensor->nSensor,std::string(name+"_camera").c_str());
}
int proxSensHandle=-1;
if (Sensor->proximitySensorPresent)
{ // Proximity sensors seem to be very very specific and not general / generic at all. How come?! I.e. a succession of ray description (with min/max distances) would do
int intParams[8]={16,16,1,4,16,1,0,0};
float floatParams[15]={0.0f,0.48f,0.1f,0.1f,0.1f,0.1f,0.0f,0.02f,0.02f,30.0f*piValue/180.0f,piValue/2.0f,0.0f,0.02f,0.0f,0.0f};
proxSensHandle=simCreateProximitySensor(sim_proximitysensor_cone_subtype,sim_objectspecialproperty_detectable_all,0,intParams,floatParams,NULL);
//Set the name:
setVrepObjectName(proxSensHandle,std::string(Sensor->name+"_proximity").c_str());
}
// the doc doesn't state if a vision and proximity sensor can be declared at the same time...
if (proxSensHandle!=-1)
{
if (Sensor->nSensor!=-1)
{
Sensor->nSensorAux=proxSensHandle;
simSetObjectParent(Sensor->nSensorAux,Sensor->nSensor,true);
}
else
Sensor->nSensor=proxSensHandle;
}
// Find the local configuration:
C7Vector sensorLocal;
sensorLocal.X.set(Sensor->origin_xyz);
sensorLocal.Q=getQuaternionFromRpy(Sensor->origin_rpy);
C4Vector rot(0.0f,0.0f,piValue); // the V-REP sensors are rotated by 180deg around the Z-axis
sensorLocal.Q=sensorLocal.Q*rot;
// We attach the sensor to a link:
C7Vector x;
x.setIdentity();
int parentLinkIndex=getLinkPosition(Sensor->parentLink);
if (parentLinkIndex!=-1)
{
int parentJointLinkIndex=getJointPosition(vLinks.at(parentLinkIndex)->parent);
if (parentJointLinkIndex!=-1)
x=vJoints.at(parentJointLinkIndex)->jointBaseFrame;
}
C7Vector sensorGlobal(x*sensorLocal);
if (Sensor->nSensor!=-1)
{
simSetObjectPosition(Sensor->nSensor,-1,sensorGlobal.X.data);
simSetObjectOrientation(Sensor->nSensor,-1,sensorGlobal.Q.getEulerAngles().data);
}
if ((parentLinkIndex!=-1)&&(Sensor->nSensor!=-1))
{
if (vLinks.at(parentLinkIndex)->visuals.size()!=0)
simSetObjectParent(Sensor->nSensor,vLinks.at(parentLinkIndex)->nLinkVisual,true);
if (vLinks.at(parentLinkIndex)->nLinkCollision!=-1)
simSetObjectParent(Sensor->nSensor,vLinks.at(parentLinkIndex)->nLinkCollision,true);
}
}
}
}
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);
}
}
void splash(){
textColor = LIGHTRED;
printToConsole(spashscreen);
textColor = LIGHTGRAY;
}
/* Programs.c */
void NYI(){
printToConsole("Error: Command not yet implemented!\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
}
}
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");
}
