コード例 #1
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setJointForceCallback(const std_msgs::Float64::ConstPtr& force)
{
	if (_handleGeneralCallback_before())
	{
		if (simSetJointForce(auxInt1,(float)force->data)==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #2
0
ファイル: vrepSubscriber.cpp プロジェクト: air-lasca/air1
void CSubscriberData::setJointStateCallback(const vrep_common::JointSetStateData::ConstPtr& data)
{
	if ( (data->handles.data.size()>0)&&(data->handles.data.size()==data->setModes.data.size())&&(data->handles.data.size()==data->values.data.size()) )
	{
		for (unsigned int i=0;i<data->handles.data.size();i++)
		{
			int handle=data->handles.data[i];
			unsigned char setMode=data->setModes.data[i];
			float val=data->values.data[i];
			if (setMode==0)
				simSetJointPosition(handle,val);
			if (setMode==1)
				simSetJointTargetPosition(handle,val);
			if (setMode==2)
				simSetJointTargetVelocity(handle,val);
			if (setMode==3)
				simSetJointForce(handle,val);
		}
	}
}
コード例 #3
0
void JointControlPluglet::torqueCallback(
		const brics_actuator::JointTorques msg) {



	for (unsigned int i = 0; i < msg.torques.size(); i++) {

		if (!controlledJoint(msg.torques[i].joint_uri)) {
			continue;
		}
		int handle = simGetObjectHandle(msg.torques[i].joint_uri.c_str());

		if (handle > 0) {

			simSetObjectIntParameter(handle, 2000, 1);
			simSetObjectIntParameter(handle, 2001, 0);
			simSetObjectIntParameter(handle, 1000, 2);


			simSetJointForce(handle,
					msg.torques[i].value);
		}
	}
}
コード例 #4
0
ファイル: robot.cpp プロジェクト: RhobanProject/Vrep
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);
	}
}