Ejemplo n.º 1
0
int main(int argc,char* argv[])
{
	int portNb=0;
	int leftMotorHandle;
	int rightMotorHandle;
	int sensorHandle;

	if (argc>=5)
	{
		portNb=atoi(argv[1]);
		leftMotorHandle=atoi(argv[2]);
		rightMotorHandle=atoi(argv[3]);
		sensorHandle=atoi(argv[4]);
	}
	else
	{
		printf("Indicate following arguments: 'portNumber leftMotorHandle rightMotorHandle sensorHandle'!\n");
		extApi_sleepMs(5000);
		return 0;
	}

	int clientID=simxStart((simxChar*)"127.0.0.1",portNb,true,true,2000,5);
	if (clientID!=-1)
	{
		int driveBackStartTime=-99000;
		float motorSpeeds[2];

		while (simxGetConnectionId(clientID)!=-1)
		{
			simxUChar sensorTrigger=0;
			if (simxReadProximitySensor(clientID,sensorHandle,&sensorTrigger,NULL,NULL,NULL,simx_opmode_streaming)==simx_return_ok)
			{ // We succeeded at reading the proximity sensor
				int simulationTime=simxGetLastCmdTime(clientID);
				if (simulationTime-driveBackStartTime<3000)
				{ // driving backwards while slightly turning:
					motorSpeeds[0]=-3.1415f*0.5f;
					motorSpeeds[1]=-3.1415f*0.25f;
				}
				else
				{ // going forward:
					motorSpeeds[0]=3.1415f;
					motorSpeeds[1]=3.1415f;
					if (sensorTrigger)
						driveBackStartTime=simulationTime; // We detected something, and start the backward mode
				}
				simxSetJointTargetVelocity(clientID,leftMotorHandle,motorSpeeds[0],simx_opmode_oneshot);			
				simxSetJointTargetVelocity(clientID,rightMotorHandle,motorSpeeds[1],simx_opmode_oneshot);			
			}
			extApi_sleepMs(5);
		}
		simxFinish(clientID);
	}
	return(0);
}
Ejemplo n.º 2
0
void RobotVREP::trackConnection()
{
	int aux_clientID = simxGetConnectionId(clientID);

	if (aux_clientID == -1)
	{
		clog << "ERROR: The client is not connected to VREP. The program ends" << endl;
		delete this;
		exit(EXIT_FAILURE);
	}
	// else if (aux_clientID != clientID)
	// 	clog << "WARNING: Exist temporary disconections in-between" << endl;
}
Ejemplo n.º 3
0
int RobotVREP::getConnectionId()
{
	return simxGetConnectionId(clientID);
}
int main(int argc, char* argv[])
{
	int portNb=0;
	int motorHandles[7];
	int motorHandles2[7];
	if (argc>=9+7)
	{ // We get the port and the motor handles (for the visual feedback, not for IK!) via command-line arguments
		portNb=atoi(argv[1]);
		motorHandles[0]=atoi(argv[2]);
		motorHandles[1]=atoi(argv[3]);
		motorHandles[2]=atoi(argv[4]);
		motorHandles[3]=atoi(argv[5]);
		motorHandles[4]=atoi(argv[6]);
		motorHandles[5]=atoi(argv[7]);
		motorHandles[6]=atoi(argv[8]);
		motorHandles2[0]=atoi(argv[9]);
		motorHandles2[1]=atoi(argv[10]);
		motorHandles2[2]=atoi(argv[11]);
		motorHandles2[3]=atoi(argv[12]);
		motorHandles2[4]=atoi(argv[13]);
		motorHandles2[5]=atoi(argv[14]);
		motorHandles2[6]=atoi(argv[15]);
	}
	else
	{
		printf("Indicate following arguments: 'portNumber motor1Handle motor2Handle .. motor7Handle'!\n");
		extApi_sleepMs(5000);
		return 0;
	}

	// Read the kinematic file:
	FILE *file;
	file=fopen("lbr_iiwa_7_r800.ik","rb");
	unsigned char* data=NULL;
	int dataLength=0;
	if (file)
	{
		fseek(file,0,SEEK_END);
		unsigned long fl=ftell(file);
		dataLength=(int)fl;
		fseek(file,0,SEEK_SET);
		data=new unsigned char[dataLength];
		fread((char*)data,dataLength,1,file);
		fclose(file);
	}
	else
	{
		printf("The kinematic content file 'lbr.ik' could not be read!\n");
		extApi_sleepMs(5000);
		return 0;
	}

	// Initialize the embedded robot model1:
	int handle1=simEmbLaunch();
	simEmbStart(data,dataLength);

	// Initialize the embedded robot model2:
	int handle2=simEmbLaunch();
	simEmbStart(data,dataLength);

	delete[] data;
	simEmbSwitch(handle1); // use robot model1

	// Connect to V-REP at the above specified port, via the remote API. V-REP is just used for visual feed-back, not IK calculation!
	int clientID=simxStart("127.0.0.1",portNb,true,true,2000,5);
	if (clientID!=-1)
	{
		float simulationStep;
		simxGetFloatingParameter(clientID,sim_floatparam_simulation_time_step,&simulationStep,simx_opmode_streaming);

		simxSynchronous(clientID,1); // We enable the synchronous mode, so that we can trigger each simulation step from here

		int embeddedModelMotorHandles[7];
		embeddedModelMotorHandles[0]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint1");
		embeddedModelMotorHandles[1]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint2");
		embeddedModelMotorHandles[2]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint3");
		embeddedModelMotorHandles[3]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint4");
		embeddedModelMotorHandles[4]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint5");
		embeddedModelMotorHandles[5]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint6");
		embeddedModelMotorHandles[6]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint7");
		int embeddedModelTargetHandle=simEmbGetObjectHandle("LBR_iiwa_7_R800_target");
		int embeddedModelBaseHandle=simEmbGetObjectHandle("LBR_iiwa_7_R800");

		extIkReal v=0.0;

		// Get the initial target dummy matrix, of the embedded model:
		extIkReal matrix[12];
		simEmbGetObjectMatrix(embeddedModelTargetHandle,embeddedModelBaseHandle,matrix);

		int embeddedModelMotorHandles2[7];
		embeddedModelMotorHandles2[0]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint1");
		embeddedModelMotorHandles2[1]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint2");
		embeddedModelMotorHandles2[2]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint3");
		embeddedModelMotorHandles2[3]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint4");
		embeddedModelMotorHandles2[4]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint5");
		embeddedModelMotorHandles2[5]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint6");
		embeddedModelMotorHandles2[6]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint7");
		int embeddedModelTargetHandle2=simEmbGetObjectHandle("LBR_iiwa_7_R800_target");
		int embeddedModelBaseHandle2=simEmbGetObjectHandle("LBR_iiwa_7_R800");

		extIkReal v2=0.0;

		// Get the initial target dummy matrix, of the embedded model:
		extIkReal matrix2[12];
		simEmbGetObjectMatrix(embeddedModelTargetHandle2,embeddedModelBaseHandle2,matrix2);


		while (simxGetConnectionId(clientID)!=-1)
		{
			
			// Following 3 commands will slow down the simulation, but garantee that if the simulation time step was changed,
			// that there won't be any jumps. Following 3 commands are not needed if you don't modify the simulation time step
			// (i.e. dt) during simulation.
			simxUChar vrepWaitingForTrigger=0;
			while ( (vrepWaitingForTrigger==0)&&(simxGetConnectionId(clientID)!=-1) )
				simxGetBooleanParameter(clientID,sim_boolparam_waiting_for_trigger,&vrepWaitingForTrigger,simx_opmode_oneshot_wait);

			simxGetFloatingParameter(clientID,sim_floatparam_simulation_time_step,&simulationStep,simx_opmode_buffer);
			v+=extIkReal(0.2)*extIkReal(simulationStep);
			v2+=extIkReal(0.28)*extIkReal(simulationStep);


			simEmbSwitch(handle1); // use robot model1
			// Set the desired tip matrix by setting the target dummy matrix:
			matrix[3]=extIkReal(-0.3-cos(v)*0.1);
			matrix[7]=extIkReal(sin(v)*0.1);
			matrix[11]=extIkReal(0.629+sin(v*9)*0.01);
			simEmbSetObjectMatrix(embeddedModelTargetHandle,embeddedModelBaseHandle,matrix);

			// calculate IK:
			simEmbHandleIkGroup(sim_handle_all);

			// Read the corresponding motor angles and send them to V-REP:
			simxPauseCommunication(clientID,1); // Temporarily pause the remote API communication, in order to send all following commands at once
			extIkReal pos;
			simEmbGetJointPosition(embeddedModelMotorHandles[0],&pos);
			simxSetJointPosition(clientID,motorHandles[0],(float)pos,simx_opmode_oneshot);
			simEmbGetJointPosition(embeddedModelMotorHandles[1],&pos);
			simxSetJointPosition(clientID,motorHandles[1],(float)pos,simx_opmode_oneshot);
			simEmbGetJointPosition(embeddedModelMotorHandles[2],&pos);
			simxSetJointPosition(clientID,motorHandles[2],(float)pos,simx_opmode_oneshot);
			simEmbGetJointPosition(embeddedModelMotorHandles[3],&pos);
			simxSetJointPosition(clientID,motorHandles[3],(float)pos,simx_opmode_oneshot);
			simEmbGetJointPosition(embeddedModelMotorHandles[4],&pos);
			simxSetJointPosition(clientID,motorHandles[4],(float)pos,simx_opmode_oneshot);
			simEmbGetJointPosition(embeddedModelMotorHandles[5],&pos);
			simxSetJointPosition(clientID,motorHandles[5],(float)pos,simx_opmode_oneshot);
			simEmbGetJointPosition(embeddedModelMotorHandles[6],&pos);
			simxSetJointPosition(clientID,motorHandles[6],(float)pos,simx_opmode_oneshot);

			simEmbSwitch(handle2); // use robot model2
			// Set the desired tip matrix by setting the target dummy matrix:
			matrix2[3]=extIkReal(-0.3-cos(v2)*0.1);
			matrix2[7]=extIkReal(sin(v2)*0.1);
			simEmbSetObjectMatrix(embeddedModelTargetHandle2,embeddedModelBaseHandle2,matrix2);

			// calculate IK:
			simEmbHandleIkGroup(sim_handle_all);

			// Read the corresponding motor angles and send them to V-REP:
			simEmbGetJointPosition(embeddedModelMotorHandles2[0],&pos);
			simxSetJointPosition(clientID,motorHandles2[0],(float)pos,simx_opmode_oneshot);
			simEmbGetJointPosition(embeddedModelMotorHandles2[1],&pos);
			simxSetJointPosition(clientID,motorHandles2[1],(float)pos,simx_opmode_oneshot);
			simEmbGetJointPosition(embeddedModelMotorHandles2[2],&pos);
			simxSetJointPosition(clientID,motorHandles2[2],(float)pos,simx_opmode_oneshot);
			simEmbGetJointPosition(embeddedModelMotorHandles2[3],&pos);
			simxSetJointPosition(clientID,motorHandles2[3],(float)pos,simx_opmode_oneshot);
			simEmbGetJointPosition(embeddedModelMotorHandles2[4],&pos);
			simxSetJointPosition(clientID,motorHandles2[4],(float)pos,simx_opmode_oneshot);
			simEmbGetJointPosition(embeddedModelMotorHandles2[5],&pos);
			simxSetJointPosition(clientID,motorHandles2[5],(float)pos,simx_opmode_oneshot);
			simEmbGetJointPosition(embeddedModelMotorHandles2[6],&pos);
			simxSetJointPosition(clientID,motorHandles2[6],(float)pos,simx_opmode_oneshot);

			simxPauseCommunication(clientID,0); // Unpause the remote API communication

			// Now step the simulation on V-REP side:
			int r=simx_return_remote_error_flag; // means for next remote API function call: step not triggered
			while (r==simx_return_remote_error_flag)
				r=simxSynchronousTrigger(clientID); // Trigger next simulation step
			if (r!=simx_return_ok)
				break;

			printf(".");
		}
		simEmbShutDown(); // End the external IK
		simEmbShutDown(); // End the external IK
		simxFinish(clientID); // End the remote API
	}
	return(0);
}
Ejemplo n.º 5
-1
int main(int argc,char* argv[])
{
	int portNb=0;
	int leftMotorHandle;
	int rightMotorHandle;
	int cuboidHandle;
	int goalHandle;

	if (argc>=6)
	{
		portNb=atoi(argv[1]);
		leftMotorHandle=atoi(argv[2]);
		rightMotorHandle=atoi(argv[3]);
		cuboidHandle=atoi(argv[4]);
		goalHandle=atoi(argv[5]);
	}
	else
	{
		extApi_sleepMs(5000);
		return 0;
	}

	int clientID=simxStart((simxChar*)"127.0.0.1",portNb,true,true,2000,5);
	if (clientID!=-1)
	{
		int driveBackStartTime=-99000;
		float motorSpeeds[2];
		float leftMotorAngle;
		float ObjectPosition[3];
		float GoalPosition[3];
		float ObjectOrientation[3];
		float minDistance=0.3;

		if (simxGetConnectionId(clientID)!=-1)
		{  
			simxUChar sensorTrigger=0;

			simxGetObjectHandle(clientID, argv[2], &leftMotorHandle, simx_opmode_blocking);
            simxGetObjectHandle(clientID, argv[3], &rightMotorHandle, simx_opmode_blocking);
			simxGetObjectHandle(clientID, argv[4], &cuboidHandle, simx_opmode_blocking);
			simxGetObjectHandle(clientID, argv[5], &goalHandle, simx_opmode_blocking);


			simxGetObjectPosition(clientID,goalHandle,-1,GoalPosition,simx_opmode_oneshot_wait);
			MovetoPoint(GoalPosition, minDistance, clientID, leftMotorHandle, rightMotorHandle, cuboidHandle);
			simxSetJointTargetVelocity(clientID,leftMotorHandle,0,simx_opmode_blocking);			
      		simxSetJointTargetVelocity(clientID,rightMotorHandle,0,simx_opmode_blocking);
			extApi_sleepMs(5);

		}
		printf("Fin!\n");
		simxFinish(clientID);
	}
	return(0);
}