Exemplo n.º 1
0
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;
}
Exemplo n.º 2
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
	}
}
Exemplo n.º 3
0
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);
				}
			}
		}
}
Exemplo n.º 4
0
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);
			}
		}
}
Exemplo n.º 5
0
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);
	}
}
// This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages):
VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
{   // This is called quite often. Just watch out for messages/events you want to handle
    // Keep following 5 lines at the beginning and unchanged:
    simLockInterface(1);
    int errorModeSaved;
    simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
    simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
    void* retVal=NULL;

    // Here we can intercept many messages from V-REP (actually callbacks). Only the most important messages are listed here:

    if (message==sim_message_eventcallback_instancepass)
    {   // This message is sent each time the scene was rendered (well, shortly after) (very often)
        // When a simulation is not running, but you still need to execute some commands, then put some code here
    }

    if (message==sim_message_eventcallback_mainscriptabouttobecalled)
    {   // Main script is about to be run (only called while a simulation is running (and not paused!))
        //
        // This is a good location to execute commands (e.g. commands needed to generate ROS messages)
        //
        // e.g. to read all joint positions:
        simLockInterface(1);

        ros::spinOnce();

        sensor_msgs::Image image_msg;

        //
        // Execute player speceific functions
        //
        if ((player_turn_state == PLAYER_NONE) && new_player_turn)
        {
            //ROS_INFO("New player turn received: %f %f", player_turn.power, player_turn.angle);
            ROS_INFO("[PLAYER_NONE] Player turn ended.\n");
            new_player_turn = false;
        }
        else if (player_turn_state == PLAYER_ROTATE_MOVE)
        {
            tip_position[0] = table_state.white_ball.x - TIP_OFFSET_DISTANCE * sin(player_turn.angle);
            tip_position[1] = table_state.white_ball.y + TIP_OFFSET_DISTANCE * cos(player_turn.angle);

            tip_orientation[2] = player_turn.angle;

            std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_ROTATE_MOVE] Orientation success: " <<
                      simSetObjectOrientation(tip_handle, WORLD_FRAME, tip_orientation) << std::endl;
            std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_ROTATE_MOVE] Position success: " <<
                      simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl;

            player_turn_state = PLAYER_CUE_DOWN;
        }
        else if (player_turn_state == PLAYER_CUE_DOWN)
        {
            tip_position[2] = CUE_ON_TABLE_HEIGHT;
            std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_CUE_DOWN] Position success: " <<
                      simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl;

            player_turn_state = PLAYER_EXECUTE_SHOT;
        }
        else if (player_turn_state == PLAYER_EXECUTE_SHOT)
        {
            double dist = player_turn.power * POWER_DISTANCE_RATIO + MIN_POWER_DISTANCE;
            tip_position[0] += sin(player_turn.angle) * dist;
            tip_position[1] -= cos(player_turn.angle) * dist;

            std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_EXECUTE_SHOT] Position success: " <<
                      simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl;

            player_turn_state = PLAYER_RAISE_CUE;
        }
        else if (player_turn_state == PLAYER_RAISE_CUE)
        {
            tip_position[2] = CUE_RAISED_HEIGHT;
            std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_RAISE_CUE] Position success: " <<
                      simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl;

            player_turn_state = PLAYER_NONE;
        }

        generate_image_message(image_msg);

        //
        // Transfer to old table state and get new state
        //
        old_table_state = table_state;
        get_ball_positions();
        get_cue_position_orientation();

        //
        //Check if table is in steady state
        //
        steady_state = (table_state.white_ball.z < POOL_TABLE_HEIGHT)?
                       (true):(closeTo(old_table_state.white_ball, table_state.white_ball, STEADY_STATE_DRIFT));
        for (unsigned int i = 0; (i < BALL_COUNT) && steady_state; i++)
        {
            if (!closeTo(old_table_state.balls[i], table_state.balls[i], STEADY_STATE_DRIFT))
            {
                steady_state = false;
            }
        }

        //
        // Publish information
        //
        if (steady_state && (player_turn_state == PLAYER_NONE && new_player_turn == false))
        {
            ROS_INFO("Table is in steady state!\n");
            //calculate turn score and publish it.
            int score = calculate_turn_score();
            ROS_INFO("The score for the last turn is %d", score);
            std_msgs::Int32 score_msg;
            score_msg.data = score;
            score_pub.publish(score_msg);
            usleep(50 * 1000); //50ms
            table_state_pub.publish(table_state); // publish a message only if nothing is moving
        }

        //
        // Move white ball if potted
        //
        if (steady_state && (table_state.white_ball.z < POOL_TABLE_HEIGHT)&&
                (player_turn_state == PLAYER_NONE && new_player_turn == false))
        {
            std::cout << "[V-REP_ROS_POOL_SIM] Reset: " << simResetDynamicObject(white_ball_handle) << " ";
            std::cout << "[V-REP_ROS_POOL_SIM] Moving white to initial position success: " <<
                      simSetObjectPosition(white_ball_handle, WORLD_FRAME, white_ball_initial_position) << std::endl;
            table_state.white_ball.x = white_ball_initial_position[0];
            table_state.white_ball.y = white_ball_initial_position[1];
            table_state.white_ball.z = white_ball_initial_position[2];
        }

        //
        // Move all red balls is all potted
        //
        if (steady_state)
        {
            int balls_in = 0;
            for (unsigned int i = 0; i < BALL_COUNT; i++)
            {
                if (table_state.balls[i].z < POOL_TABLE_HEIGHT)
                {
                    balls_in++;
                }
            }
            if (balls_in == BALL_COUNT)
            {
                //Move all balls to the top!
                std::cout << "[V-REP_ROS_POOL_SIM] Reset: " << simResetDynamicObject(white_ball_handle) << " ";
                std::cout << "[V-REP_ROS_POOL_SIM] Moving white to initial position success: " <<
                          simSetObjectPosition(white_ball_handle, WORLD_FRAME, white_ball_initial_position) << std::endl;

                for (unsigned int i = 0; i < BALL_COUNT; i++)
                {
                    std::cout << "[V-REP_ROS_POOL_SIM] Ball " << i << std::endl;
                    float p[3];
                    p[0] = initial_table_state.balls[i].x;
                    p[1] = initial_table_state.balls[i].y;
                    p[2] = initial_table_state.balls[i].z;

                    std::cout << "[V-REP_ROS_POOL_SIM] Reset: " << simResetDynamicObject(balls_handle[i]) << std::endl;
                    std::cout << "[V-REP_ROS_POOL_SIM] Moving ball position success: " <<
                              simSetObjectPosition(balls_handle[i], WORLD_FRAME, p) << std::endl;
                }
                table_state = initial_table_state;
                games_played++;
                ROS_INFO("The are %d games played so far.", games_played);
            }
        }

        simLockInterface(0);

        //Publish messages
        img_pub.publish(image_msg);

        // The best would be to start ROS activity when a simulation is running, and stop it when a simulation ended
        // If you allow ROS activity while a simulation is not running, then it becomes a little bit tricky when a scene
        // was switched for example (e.g. the clients would need a way to detect that)
    }

    if (message==sim_message_eventcallback_simulationabouttostart)
    {   // Simulation is about to start
        // Here you could launch the ROS node (if using just one node)
        // If more than one node should be supported (e.g. with different services per node),
        // then it is better to start a node via a custom Lua function
        white_ball_handle = simGetObjectHandle("white_ball");
        std::cout << "[V-REP POOL PLUGIN] White ball handle: " << white_ball_handle << std::endl;
        tip_handle = simGetObjectHandle("tip");

        for (int i=0; i < BALL_COUNT; i++)
        {
            std::stringstream ball_name;
            ball_name << "ball";
            ball_name << i;
            balls_handle[i] = simGetObjectHandle(ball_name.str().c_str());
            //std::cout << "Ball " << i << " handle: " << balls_handle[i] << std::endl;
        }
        camera_handle = simGetObjectHandle("camera_sensor");
        std::cout << "[V-REP POOL PLUGIN] Camera handle: " << camera_handle << std::endl;

        //Read all positions and make them as old positions
        get_ball_positions();
        old_table_state = table_state;
        pre_hit_table_state = table_state;
        initial_table_state = table_state;
    }

    if (message==sim_message_eventcallback_simulationended)
    {   // Simulation just ended
        // Here you could kill the ROS node(s) that are still active. There could also be a custom Lua function to kill a specific ROS node.
    }

    // Keep following unchanged:
    simSetIntegerParameter(sim_intparam_error_report_mode, errorModeSaved); // restore previous settings
    simLockInterface(0);
    return (retVal);
}