Beispiel #1
0
void CSubscriberData::setObjectSelectionCallback(const std_msgs::Int32MultiArray::ConstPtr& objHandles)
{
	simRemoveObjectFromSelection(sim_handle_all,0);
	int cnt=objHandles->data.size();
	for (int i=0;i<cnt;i++)
		simAddObjectToSelection(sim_handle_single,objHandles->data[i]);
}
Beispiel #2
0
void CBubbleRobDialog::OnLbnSelchangeBubblerobList()
{
	simLockInterface(1);
	int objectHandle=getSelectedObjectInList();
	simRemoveObjectFromSelection(sim_handle_all,-1);
	CBubbleRob* bubbleRob=CAccess::bubbleRobContainer->getFromID(objectHandle);
	if (bubbleRob!=NULL)
	{
		int sceneObjHandle=bubbleRob->getAssociatedObject();
		if (sceneObjHandle!=-1)
			simAddObjectToSelection(sim_handle_single,sceneObjHandle);
	}
	simLockInterface(0);
}
Beispiel #3
0
void CMtbRobotDialog::on_qqRobotList_itemSelectionChanged()
{
	if (!inRefreshRoutine)
	{
		simLockInterface(1);
		int objectHandle=getSelectedObjectInList();
		simRemoveObjectFromSelection(sim_handle_all,-1);
		CMtbRobot* mtbRobot=CAccess::mtbRobotContainer->getFromID(objectHandle);
		if (mtbRobot!=NULL)
		{
			int sceneObjHandle=mtbRobot->getAssociatedObject();
			if (sceneObjHandle!=-1)
				simAddObjectToSelection(sim_handle_single,sceneObjHandle); // select the associated scene object!
		}
		simLockInterface(0);
	}
}
Beispiel #4
0
//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
	}
}
Beispiel #5
0
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");
}