示例#1
0
void CBubbleRob::handleSimulation()
{ // called when the main script calls: simHandleModule
	_initialize();	
	if (_movementDuration>0.0f)
	{ // movement mode
		if (simReadProximitySensor(_handleOfProximitySensor,NULL,NULL,NULL)>0)
			_backMovementDuration=3.0f; // we detected an obstacle, we move backward for 3 seconds
		if (_backMovementDuration>0.0f)
		{ // We move backward
			simSetJointTargetVelocity(_handleOfLeftJoint,-_maxVelocity*_backRelativeVelocities[0]);
			simSetJointTargetVelocity(_handleOfRightJoint,-_maxVelocity*_backRelativeVelocities[1]);
			_backMovementDuration-=simGetSimulationTimeStep();
		}
		else
		{ // We move forward
			simSetJointTargetVelocity(_handleOfLeftJoint,_maxVelocity);
			simSetJointTargetVelocity(_handleOfRightJoint,_maxVelocity);
		}
		_movementDuration-=simGetSimulationTimeStep();
		if ((_movementDuration<=0.0f)&&(_setToZeroAtMovementEnd!=NULL))
		{
			_setToZeroAtMovementEnd[0]=0;	// This will unblock the thread that called simExtBubbleMoveAndAvoid(x,y,false)
											// Above memory location can only be manipulated if we are sure the simulation is still running!!
			_setToZeroAtMovementEnd=NULL;
		}
	}
	else
	{ // stopped mode
		simSetJointTargetVelocity(_handleOfLeftJoint,0.0f);
		simSetJointTargetVelocity(_handleOfRightJoint,0.0f);
	}
}
void SetTwistHandler::handleSimulation(){
    // called when the main script calls: simHandleModule
    if(!_initialized){
        _initialize();
    }

    Eigen::Quaternion < simFloat > orientation; //(x,y,z,w)
    Eigen::Matrix< simFloat, 3, 1> linVelocity((simFloat)_twistCommands.twist.linear.x,
    		(simFloat)_twistCommands.twist.linear.y,
    		(simFloat)_twistCommands.twist.linear.z);
    Eigen::Matrix< simFloat, 3, 1> angVelocity((simFloat)_twistCommands.twist.angular.x,
    		(simFloat)_twistCommands.twist.angular.y,
    		(simFloat)_twistCommands.twist.angular.z);

    // Input velocity is expected to be in body frame but V-Rep expects it to be in world frame.
    if(simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){
        linVelocity = orientation*linVelocity;
	    angVelocity = orientation*angVelocity;
    } else {
        std::stringstream ss;
        ss << "- [" << _associatedObjectName << "] Error getting orientation. " << std::endl;
        ConsoleHandler::printInConsole(ss);
    }

    simResetDynamicObject(_associatedObjectID);

	//Set object velocity
    if (_isStatic){
    	Eigen::Matrix<simFloat, 3, 1> position;
    	simGetObjectPosition(_associatedObjectID, -1, position.data());
    	const simFloat timeStep = simGetSimulationTimeStep();
    	position = position + timeStep*linVelocity;
    	simSetObjectPosition(_associatedObjectID, -1, position.data());
    	const simFloat angle = timeStep*angVelocity.norm();
    	if(angle > 1e-6){
    		orientation = Eigen::Quaternion< simFloat >(Eigen::AngleAxis< simFloat >(timeStep*angVelocity.norm(), angVelocity/angVelocity.norm()))*orientation;
    	}
    	simSetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data());
    } else {
		// Apply the linear velocity to the object
		if(simSetObjectFloatParameter(_associatedObjectID, 3000, linVelocity[0])
			&& simSetObjectFloatParameter(_associatedObjectID, 3001, linVelocity[1])
			&& simSetObjectFloatParameter(_associatedObjectID, 3002, linVelocity[2])==-1) {
			std::stringstream ss;
			ss << "- [" << _associatedObjectName << "] Error setting linear velocity. " << std::endl;;
			ConsoleHandler::printInConsole(ss);
		}
		// Apply the angular velocity to the object
		if(simSetObjectFloatParameter(_associatedObjectID, 3020, angVelocity[0])
			&& simSetObjectFloatParameter(_associatedObjectID, 3021, angVelocity[1])
			&& simSetObjectFloatParameter(_associatedObjectID, 3022, angVelocity[2])==-1) {
			std::stringstream ss;
			ss << "- [" << _associatedObjectName << "] Error setting angular velocity. " << std::endl;;
			ConsoleHandler::printInConsole(ss);
		}
    }

}
示例#3
0
// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
	// Dynamically load and bind V-REP functions:
	// ******************************************
	// 1. Figure out this plugin's directory:
	char curDirAndFile[1024];
#ifdef _WIN32
	GetModuleFileName(NULL,curDirAndFile,1023);
	PathRemoveFileSpec(curDirAndFile);
#elif defined (__linux) || defined (__APPLE__)
	getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
	std::string currentDirAndPath(curDirAndFile);
	// 2. Append the V-REP library's name:
	std::string temp(currentDirAndPath);
#ifdef _WIN32
	temp+="\\v_rep.dll";
#elif defined (__linux)
	temp+="/libv_rep.so";
#elif defined (__APPLE__)
	temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
	// 3. Load the V-REP library:
	vrepLib=loadVrepLibrary(temp.c_str());
	if (vrepLib==NULL)
	{
		std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'RTCSkeleton' plugin.\n";
		return(0); // Means error, V-REP will unload this plugin
	}
	if (getVrepProcAddresses(vrepLib)==0)
	{
		std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'RTCSkeleton' plugin.\n";
		unloadVrepLibrary(vrepLib);
		return(0); // Means error, V-REP will unload this plugin
	}
	// ******************************************

	// Check the version of V-REP:
	// ******************************************
	int vrepVer;
	simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
	if (vrepVer<20604) // if V-REP version is smaller than 2.06.04
	{
		std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'RTCSkeleton' plugin.\n";
		unloadVrepLibrary(vrepLib);
		return(0); // Means error, V-REP will unload this plugin
	}
	// ******************************************

	// Here you could handle various initializations
	// Here you could also register custom Lua functions or custom Lua constants
	// etc.

	const simChar* entryLabel = "";
	simInt itemCount = 1;
//	simAddModuleMenuEntry(entryLabel, itemCount, &mainItemHandle);
//	simSetModuleMenuItemState(mainItemHandle, 1, "RTCPlugin");

	entryLabel = "RTCPlugin";
	itemCount = 1;
	simAddModuleMenuEntry(entryLabel, itemCount, &robotItemHandle);
	simSetModuleMenuItemState(robotItemHandle, 1, "Add Robot RTC");

	simAddModuleMenuEntry(entryLabel, itemCount, &rangeItemHandle);
	simSetModuleMenuItemState(rangeItemHandle, 1, "Add Range RTC");

	simAddModuleMenuEntry(entryLabel, itemCount, &cameraItemHandle);
	simSetModuleMenuItemState(cameraItemHandle, 1, "Add Camera RTC");

	simAddModuleMenuEntry(entryLabel, itemCount, &accelItemHandle);
	simSetModuleMenuItemState(accelItemHandle, 1, "Add Acceleration RTC");

	simAddModuleMenuEntry(entryLabel, itemCount, &gyroItemHandle);
	simSetModuleMenuItemState(gyroItemHandle, 1, "Add Gyro RTC");

	simAddModuleMenuEntry(entryLabel, itemCount, &depthItemHandle);
	simSetModuleMenuItemState(depthItemHandle, 1, "Add Depth RTC");

	simAddModuleMenuEntry(entryLabel, itemCount, &objectItemHandle);
	simSetModuleMenuItemState(objectItemHandle, 1, "Add Object RTC");

	initRTM();
	simulatorClock.setSimulationTimeStep(simGetSimulationTimeStep());

	std::cout << "v_repExtRTC plugin safely loaded.\n";

	return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
示例#4
0
// 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:
        //static bool refreshDlgFlag=true;
	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.
	// For a complete list of messages that you can intercept/react with, search for "sim_message_eventcallback"-type constants
	// in the V-REP user manual.
	
	if (message==sim_message_eventcallback_mainscriptabouttobecalled)
	{ // The main script is about to be run (only called while a simulation is running (and not paused!))
	  
	  //  main script is called every dynamics calculation. 
	  tickRTCs(simGetSimulationTimeStep());
	}
	if (message==sim_message_eventcallback_simulationabouttostart)
	{ // Simulation is about to start
	  simulatorClock.setSimulationTimeStep(simGetSimulationTimeStep());
	  //simulatorClock.setSimulationTime(simGetSimulationTime());
	  startRTCs();
	}
	if (message == sim_message_eventcallback_menuitemselected) {
		simInt handle = auxiliaryData[0];
		simInt state = auxiliaryData[1];
		if (handle == robotItemHandle) {
			std::cout << "Robot Menu Selected" << std::endl;
			mainItemHandle = robotItemHandle;
			spawnRTCMethod = spawnRobotRTC;
			modelDlgHandle = simDisplayDialog("Input Robot Item", "Input Robot Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
		}
		else if (handle == rangeItemHandle) {
			std::cout << "Range Menu Selected" << std::endl;
			mainItemHandle = rangeItemHandle;
			spawnRTCMethod = spawnRangeRTC;
			modelDlgHandle = simDisplayDialog("Input Item", "Input Range Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
		}
		else if (handle == cameraItemHandle) {
			std::cout << "Camera Menu Selected" << std::endl;
			mainItemHandle = cameraItemHandle;
			spawnRTCMethod = spawnCameraRTC;
			modelDlgHandle = simDisplayDialog("Input Item", "Input Vision Sensor Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
		}
		else if (handle == accelItemHandle) {
			std::cout << "Acceleration Menu Selected" << std::endl;
			mainItemHandle = accelItemHandle;
			spawnRTCMethod = spawnAccelerometerRTC;
			modelDlgHandle = simDisplayDialog("Input Item", "Input Accel Sensor Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
		}
		else if (handle == gyroItemHandle) {
			std::cout << "Gyro Menu Selected" << std::endl;
			mainItemHandle = gyroItemHandle;
			spawnRTCMethod = spawnGyroRTC;
			modelDlgHandle = simDisplayDialog("Input Item", "Input Gyro Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
		}
		else if (handle == depthItemHandle) {
			std::cout << "Depth Menu Selected" << std::endl;
			mainItemHandle = depthItemHandle;
			spawnRTCMethod = spawnDepthRTC;
			modelDlgHandle = simDisplayDialog("Input Item", "Input Depth Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
		}
		else if (handle == objectItemHandle) {
			std::cout << "Object Menu Selected" << std::endl;
			mainItemHandle = objectItemHandle;
			spawnRTCMethod = spawnObjectRTC;
			modelDlgHandle = simDisplayDialog("Input Item", "Input Object Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
		}

	}

	if (message == sim_message_eventcallback_refreshdialogs) {
		if (modelDlgHandle >= 0) {
			simInt ret = simGetDialogResult(modelDlgHandle);
			if (ret != sim_dlgret_still_open) {
				if (ret == sim_dlgret_ok) {
					char *buf = simGetDialogInput(modelDlgHandle);
					modelName = buf;
					std::cout << "Model Name is " << buf << std::endl;
					simReleaseBuffer(buf);

					argDlgHandle = simDisplayDialog("Input Argument of RTC", "Input Argument of createComponent method", sim_dlgstyle_input, "", NULL, NULL, NULL);
					modelDlgHandle = -1;
				}
			}
		} else if (argDlgHandle >= 0) {
			simInt ret = simGetDialogResult(argDlgHandle);
			if (ret != sim_dlgret_still_open) {
				if (ret == sim_dlgret_ok) {
					char *buf = simGetDialogInput(argDlgHandle);
					argument = buf;
					std::cout << "createComponent Argument is " << buf << std::endl;
					simReleaseBuffer(buf);
					argDlgHandle = -1;
					spawnRTCMethod(modelName, argument);
				}
			}
		}
	}

	if (message==sim_message_eventcallback_simulationended)
	{ // Simulation just ended
	  stopRTCs();
	}
	simulatorClock.setSimulationTime(simGetSimulationTime());
	message_pump();
	// Keep following unchanged:
	simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
	return(retVal);
}
示例#5
0
void message_pump() {
	// Task Queue Check for unsynchronized task thrown from Service Port of RT-Component.
	Task t = taskQueue.popTask();
	simInt ret;
	switch(t.value){
	case Task::START:
	  std::cout << " - Task::Starting Simulation" << std::endl;
	  ret = simStartSimulation();
	  if (ret == 0) {
	    returnQueue.returnReturn(Return(Return::RET_FAILED));
	  } else if(ret < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }
	  break;
	case Task::STOP:
	  std::cout << " - Task::Stopping Simulation" << std::endl;
	  ret = simStopSimulation();
	  if (ret == 0) {
	    returnQueue.returnReturn(Return(Return::RET_FAILED));
	  } else if(ret < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }
	  break;
	case Task::PAUSE:
	  std::cout << " - Task::Pausing Simulation" << std::endl;
	  ret = simPauseSimulation();
	  if (ret == 0) {
	    returnQueue.returnReturn(Return(Return::RET_FAILED));
	  } else if(ret < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }
	  break;
	case Task::LOADPROJECT:
	  std::cout << " - Task::Loading Project: " << t.key.c_str() << std::endl;
	  ret = simLoadScene(t.key.c_str());
	  std::cout << " -- ret = " << ret;
	  if (ret < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }
	  break;
	case Task::SPAWNROBOT:
	  std::cout << " - Task::SpawnRobot" << std::endl;
	  if (spawnRobotRTC(t.key, t.arg) < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }
	  break;
	case Task::SPAWNRANGE:
	  std::cout << " - Task::Spawnrange" << std::endl;
	  if (spawnRangeRTC(t.key, t.arg) < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }

	  break;
	case Task::SPAWNDEPTH:
	  std::cout << " - Task::Spawndepth" << std::endl;
	  if (spawnDepthRTC(t.key, t.arg) < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }

	  break;
	case Task::SPAWNACCEL:
	  std::cout << " - Task::Spawnaccel" << std::endl;
	  if (spawnAccelerometerRTC(t.key, t.arg) < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }

	  break;
	case Task::SPAWNGYRO:
	  std::cout << " - Task::Spawngyro" << std::endl;
	  if (spawnGyroRTC(t.key, t.arg) < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }

	  break;
	case Task::SPAWNCAMERA:
	  std::cout << " - Task::SpawnCamera" << std::endl;
	  if (spawnCameraRTC(t.key, t.arg) < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }

	  break;
	case Task::SPAWNOBJECT:
		std::cout << " - Task::SpawnObject" << std::endl;
		if (spawnObjectRTC(t.key, t.arg) < 0) {
			returnQueue.returnReturn(Return(Return::RET_ERROR));
		}
		else {
			returnQueue.returnReturn(Return(Return::RET_OK));
		}

		break;
	case Task::KILLRTC:
	  std::cout << " - Task::KillRTC" << std::endl;
	  if (killRTC(t.key) < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }
	  break;
	case Task::KILLALLRTC:
	  std::cout << " - Task::KillAllRTC" << std::endl;
	  if (killAllRTC() < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }
	  break;
	case Task::SYNCRTC:
	  std::cout << " - Task::SYNCRTC" << std::endl;
	  if (syncRTC(t.key) < 0) {
	    returnQueue.returnReturn(Return(Return::RET_ERROR));
	  } else {
	    returnQueue.returnReturn(Return(Return::RET_OK));
	  }
	  break;
	case Task::GETSYNCRTC:
	  std::cout << " - Task::GETSYNCRTC" << std::endl;
	  {
	    Return r(Return::RET_OK);
	    if (getSyncRTCs(r.stringList) < 0) {
	      r.value = Return::RET_ERROR;
	      returnQueue.returnReturn(r);
	    } else {
	      for(int i = 0;i < r.stringList.size();i++) {
		std::cout << " - Task: " << r.stringList[i] << std::endl;
	      }
	      returnQueue.returnReturn(r);
	    }
	  }
	  break;
	case Task::GETSIMTIME:
	  std::cout << " - Task::GETSIMTIME" << std::endl;
	  {
	    Return r(Return::RET_OK);
	    r.floatValue = simGetSimulationTime();
	    returnQueue.returnReturn(r);
	  }
	  break;
	case Task::GETSIMSTEP:
	  std::cout << " - Task::GETSIMSTEP" << std::endl;
	  {
	    Return r(Return::RET_OK);
	    r.floatValue = simGetSimulationTimeStep();
	    returnQueue.returnReturn(r);
	  }
	  break;
	case Task::GETOBJPOSE:
	  returnQueue.returnReturn(Return(Return::RET_OK));
	  break;

	case Task::SETOBJPOSE:
	  returnQueue.returnReturn(Return(Return::RET_OK));
	  break;
	  
	default:
	  //returnQueue.returnReturn(Return(Return::RET_ERROR));
	  break;
	}


}
示例#6
0
void CK3::handleSimulation()
{ // called when the main script calls: simHandleModule
	_initialize();
	float dt=simGetSimulationTimeStep();
	for (int i=0;i<2;i++)
	{
		if (_targetVelocities[i]>_currentVelocities[i])
		{
			_currentVelocities[i]=_currentVelocities[i]+_maxAcceleration*dt;
			if (_currentVelocities[i]>_targetVelocities[i])
				_currentVelocities[i]=_targetVelocities[i];
		}
		else
		{
			_currentVelocities[i]=_currentVelocities[i]-_maxAcceleration*dt;
			if (_currentVelocities[i]<_targetVelocities[i])
				_currentVelocities[i]=_targetVelocities[i];
		}
		simSetJointTargetVelocity(_k3MotorHandles[i],_currentVelocities[i]);
		float jp;
		simGetJointPosition(_k3MotorHandles[i],&jp);
		float dp=jp-_previousMotorAngles[i];
		if (fabs(dp)>piValue)
			dp-=(2.0f*piValue*fabs(dp)/dp);
//		_cumulativeMotorAngles[i]+=dp/(2.0f*piValue);
		_cumulativeMotorAngles[i]+=dp; // corrected on 5/3/2012 thanks to Felix Fischer
		_previousMotorAngles[i]=jp;
	}


	float adp=_targetArmPosition-_currentArmPosition;
	if (adp!=0.0f)
	{
		if (adp*_currentArmVelocity>=0.0f)
		{
			float decelToZeroTime=(fabs(_currentArmVelocity)+_maxArmAcceleration*dt*1.0f)/_maxArmAcceleration;
			float distToZero=0.5f*_maxArmAcceleration*decelToZeroTime*decelToZeroTime;
			float dir=1.0f;
			if (_currentArmVelocity!=0.0f)
				dir=_currentArmVelocity/fabs(_currentArmVelocity);
			else
				dir=adp/fabs(adp);
			if (fabs(adp)>distToZero)
				_currentArmVelocity+=dir*_maxArmAcceleration*dt;
			else
				_currentArmVelocity-=dir*_maxArmAcceleration*dt;
		}
		else
			_currentArmVelocity*=(1-_maxArmAcceleration*dt/fabs(_currentArmVelocity));
	}
	else
	{
		if (_currentArmVelocity!=0.0f)
		{
			float dv=-_currentArmVelocity*_maxArmAcceleration*dt/fabs(_currentArmVelocity);
			if ((_currentArmVelocity+dv)*_currentArmVelocity<0.0f)
				_currentArmVelocity=0.0f;
			else
				_currentArmVelocity+=dv;
		}
	}

	_currentArmPosition+=_currentArmVelocity*dt;

	simSetJointTargetPosition(_k3GripperArmMotorHandles[0],_currentArmPosition);
	simSetJointTargetPosition(_k3GripperArmMotorHandles[1],-_currentArmPosition);
	simSetJointTargetPosition(_k3GripperArmMotorHandles[2],_currentArmPosition);
	simSetJointTargetPosition(_k3GripperArmMotorHandles[3],_currentArmPosition);
	simSetJointTargetPosition(_k3GripperArmMotorHandles[4],_currentArmPosition);
	simSetJointTargetPosition(_k3GripperArmMotorHandles[5],_currentArmPosition);

	float jp;
	simGetJointPosition(_k3GripperFingerMotorHandles[0],&jp);
	_currentGripperGap_unscaled=(jp/_scaling)+0.04f; // Don't forget scaling!!
	float dp=_targetGripperGap_unscaled-_currentGripperGap_unscaled;
	float velToRegulate=0.0f;
	if (fabs(dp)<0.005f)
	{
		if (dp!=0.0f)
			velToRegulate=(fabs(dp)/0.005f)*0.045f*dp/fabs(dp);
	}
	else
		velToRegulate=0.045f*dp/fabs(dp);
	simSetJointTargetVelocity(_k3GripperFingerMotorHandles[0],velToRegulate*_scaling); // Don't forget scaling!!
	simSetJointTargetPosition(_k3GripperFingerMotorHandles[1],-jp*0.5f/_scaling); // Don't forget scaling!!
	simSetJointTargetPosition(_k3GripperFingerMotorHandles[2],-jp*0.5f/_scaling); // Don't forget scaling!!

	_updateStateVisualization();
}
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
	// This function should not generate any error messages:
	int errorModeSaved;
	simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
	simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);

	void* retVal=NULL;

	if (message==sim_message_eventcallback_modulehandle)
	{
		if ( (customData==NULL)||(std::string("K3").compare((char*)customData)==0) ) // is the command also meant for Khepera3?
		{
			float dt=simGetSimulationTimeStep();
			for (unsigned int k3Index=0;k3Index<allK3s.size();k3Index++)
			{
				// 1. Run the robot control:
				for (int i=0;i<2;i++)
				{
					if (allK3s[k3Index].targetVelocities[i]>allK3s[k3Index].currentVelocities[i])
					{
						allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].currentVelocities[i]+allK3s[k3Index].maxAcceleration*dt;
						if (allK3s[k3Index].currentVelocities[i]>allK3s[k3Index].targetVelocities[i])
							allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].targetVelocities[i];
					}
					else
					{
						allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].currentVelocities[i]-allK3s[k3Index].maxAcceleration*dt;
						if (allK3s[k3Index].currentVelocities[i]<allK3s[k3Index].targetVelocities[i])
							allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].targetVelocities[i];
					}
					simSetJointTargetVelocity(allK3s[k3Index].wheelMotorHandles[i],allK3s[k3Index].currentVelocities[i]);
					float jp;
					simGetJointPosition(allK3s[k3Index].wheelMotorHandles[i],&jp);
					float dp=jp-allK3s[k3Index].previousMotorAngles[i];
					if (fabs(dp)>3.1415f)
						dp-=(2.0f*3.1415f*fabs(dp)/dp);
					allK3s[k3Index].cumulativeMotorAngles[i]+=dp; // corrected on 5/3/2012 thanks to Felix Fischer
					allK3s[k3Index].previousMotorAngles[i]=jp;
				}


				float adp=allK3s[k3Index].targetArmPosition-allK3s[k3Index].currentArmPosition;
				if (adp!=0.0f)
				{
					if (adp*allK3s[k3Index].currentArmVelocity>=0.0f)
					{
						float decelToZeroTime=(fabs(allK3s[k3Index].currentArmVelocity)+allK3s[k3Index].maxArmAcceleration*dt*1.0f)/allK3s[k3Index].maxArmAcceleration;
						float distToZero=0.5f*allK3s[k3Index].maxArmAcceleration*decelToZeroTime*decelToZeroTime;
						float dir=1.0f;
						if (allK3s[k3Index].currentArmVelocity!=0.0f)
							dir=allK3s[k3Index].currentArmVelocity/fabs(allK3s[k3Index].currentArmVelocity);
						else
							dir=adp/fabs(adp);
						if (fabs(adp)>distToZero)
							allK3s[k3Index].currentArmVelocity+=dir*allK3s[k3Index].maxArmAcceleration*dt;
						else
							allK3s[k3Index].currentArmVelocity-=dir*allK3s[k3Index].maxArmAcceleration*dt;
					}
					else
						allK3s[k3Index].currentArmVelocity*=(1-allK3s[k3Index].maxArmAcceleration*dt/fabs(allK3s[k3Index].currentArmVelocity));
				}
				else
				{
					if (allK3s[k3Index].currentArmVelocity!=0.0f)
					{
						float dv=-allK3s[k3Index].currentArmVelocity*allK3s[k3Index].maxArmAcceleration*dt/fabs(allK3s[k3Index].currentArmVelocity);
						if ((allK3s[k3Index].currentArmVelocity+dv)*allK3s[k3Index].currentArmVelocity<0.0f)
							allK3s[k3Index].currentArmVelocity=0.0f;
						else
							allK3s[k3Index].currentArmVelocity+=dv;
					}
				}

				allK3s[k3Index].currentArmPosition+=allK3s[k3Index].currentArmVelocity*dt;

				simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[0],allK3s[k3Index].currentArmPosition);
				simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[1],-allK3s[k3Index].currentArmPosition);
				simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[2],allK3s[k3Index].currentArmPosition);
				simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[3],allK3s[k3Index].currentArmPosition);
				simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[4],allK3s[k3Index].currentArmPosition);
				simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[5],allK3s[k3Index].currentArmPosition);

				float jp;
				simGetJointPosition(allK3s[k3Index].fingerMotorHandles[0],&jp);
				allK3s[k3Index].currentGripperGap=(jp)+0.04f;
				float dp=allK3s[k3Index].targetGripperGap-allK3s[k3Index].currentGripperGap;
				float velToRegulate=0.0f;
				if (fabs(dp)<0.005f)
				{
					if (dp!=0.0f)
						velToRegulate=(fabs(dp)/0.005f)*0.045f*dp/fabs(dp);
				}
				else
					velToRegulate=0.045f*dp/fabs(dp);
				simSetJointTargetVelocity(allK3s[k3Index].fingerMotorHandles[0],velToRegulate); 
				simSetJointTargetPosition(allK3s[k3Index].fingerMotorHandles[1],-jp*0.5f); 
				simSetJointTargetPosition(allK3s[k3Index].fingerMotorHandles[2],-jp*0.5f);

				// 2. Update the robot's UI:
				// IR sensors on the base:
				float ptAndDist[4];
				std::string s;
				for (int i=0;i<9;i++)
				{
					if (((simGetExplicitHandling(allK3s[k3Index].irSensorHandles[i])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].irSensorHandles[i],ptAndDist,NULL,NULL)>0))
					{
						simSetUIButtonLabel(allK3s[k3Index].uiHandle,110+i,"&&Box",NULL);
						std::stringstream out;
						out << int(ptAndDist[3]*1000.0f);
						s=out.str();
						simSetUIButtonLabel(allK3s[k3Index].uiHandle,210+i,s.c_str(),NULL);
					}
					else
					{
						simSetUIButtonLabel(allK3s[k3Index].uiHandle,110+i,"",NULL);
						simSetUIButtonLabel(allK3s[k3Index].uiHandle,210+i,"",NULL);
					}
				}
				
				// UI title:
				char* objName=simGetObjectName(allK3s[k3Index].k3BaseHandle);
				if (objName!=NULL)
				{
					std::string nm(objName);
					simReleaseBuffer(objName);
					nm+=" state visualization";
					simSetUIButtonLabel(allK3s[k3Index].uiHandle,0,nm.c_str(),NULL);
				}

				// US sensors on the base:
				for (int i=0;i<5;i++)
				{
					if (((simGetExplicitHandling(allK3s[k3Index].usSensorHandles[i])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].usSensorHandles[i],ptAndDist,NULL,NULL)>0))
					{
						simSetUIButtonLabel(allK3s[k3Index].uiHandle,100+i,"&&Box",NULL);
						std::stringstream out;
						out << int(ptAndDist[3]*1000.0f);
						s=out.str();
						simSetUIButtonLabel(allK3s[k3Index].uiHandle,200+i,s.c_str(),NULL);
					}
					else
					{
						simSetUIButtonLabel(allK3s[k3Index].uiHandle,100+i,"",NULL);
						simSetUIButtonLabel(allK3s[k3Index].uiHandle,200+i,"",NULL);
					}
				}
				// Color sensors on base:
				for (int i=0;i<2;i++)
				{
					float* auxValues=NULL;
					int* auxValuesCount=NULL;
					float col[3]={0.0f,0.0f,0.0f};
					if (simReadVisionSensor(allK3s[k3Index].colorSensorHandles[i],&auxValues,&auxValuesCount)>=0)
					{
						if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15))
						{
							col[0]=auxValues[11];
							col[1]=auxValues[12];
							col[2]=auxValues[13];
						}
						simReleaseBuffer((char*)auxValues);
						simReleaseBuffer((char*)auxValuesCount);
					}
					simSetUIButtonColor(allK3s[k3Index].uiHandle,300+i,col,NULL,NULL);
				}

				// Base motor velocities and encoders:
				for (int i=0;i<2;i++)
				{
					std::stringstream out1,out2;
					out1 << std::fixed << std::setprecision(1) << allK3s[k3Index].currentVelocities[i]*180.0f/3.1415f;
					s=out1.str();
					simSetUIButtonLabel(allK3s[k3Index].uiHandle,320+i,s.c_str(),NULL);

					out2 << int(float(2764)*allK3s[k3Index].cumulativeMotorAngles[i]/(2.0f*3.1415f));
					s=out2.str();
					simSetUIButtonLabel(allK3s[k3Index].uiHandle,310+i,s.c_str(),NULL);
				}

				// Arm position:
				std::stringstream out;
				out << int((1.0f-(allK3s[k3Index].currentArmPosition*180.0f/(195.0f*3.1415f)))*600.0f+300.0f);
				s=out.str();
				simSetUIButtonLabel(allK3s[k3Index].uiHandle,120,s.c_str(),NULL);
				simSetUISlider(allK3s[k3Index].uiHandle,121,int(1000.0f*allK3s[k3Index].currentArmPosition*180.0f/(195.0f*3.1415f)));

				// Finger motors:
				out.str("");
				out << int(allK3s[k3Index].currentGripperGap*1000.0f);
				s=out.str();
				simSetUIButtonLabel(allK3s[k3Index].uiHandle,130,s.c_str(),NULL);
				simSetUISlider(allK3s[k3Index].uiHandle,131,int(0.5f+(1.0f-allK3s[k3Index].currentGripperGap/0.055f)*1000.0f));
				simSetUISlider(allK3s[k3Index].uiHandle,132,int(0.5f+(allK3s[k3Index].currentGripperGap/0.055f)*1000.0f));


				// Gripper proximity sensors:
				for (int i=0;i<2;i++)
				{
					if (((simGetExplicitHandling(allK3s[k3Index].gripperDistanceSensorHandles[i])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].gripperDistanceSensorHandles[i],ptAndDist,NULL,NULL)>0))
					{
						out.str("");
						out << int(ptAndDist[3]*1000.0f);
						s=out.str();
						simSetUIButtonLabel(allK3s[k3Index].uiHandle,133+i,s.c_str(),NULL);
					}
					else
						simSetUIButtonLabel(allK3s[k3Index].uiHandle,133+i,"",NULL);
				}
				// Gripper color sensors:
				for (int i=0;i<2;i++)
				{
					float* auxValues=NULL;
					int* auxValuesCount=NULL;
					float col[3]={0.0f,0.0f,0.0f};
					if (simReadVisionSensor(allK3s[k3Index].gripperColorSensorHandles[i],&auxValues,&auxValuesCount)>=0)
					{
						if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15))
						{
							col[0]=auxValues[11];
							col[1]=auxValues[12];
							col[2]=auxValues[13];
						}
						simReleaseBuffer((char*)auxValues);
						simReleaseBuffer((char*)auxValuesCount);
					}
					simSetUIButtonColor(allK3s[k3Index].uiHandle,135+i,col,NULL,NULL);
				}

			}
		}
	}

	if (message==sim_message_eventcallback_simulationended)
	{ // simulation ended. Destroy all Khepera3 instances:
		allK3s.clear();
	}

	simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
	return(retVal);
}
示例#8
0
    /// check out sawConstraintController
    void updateKinematics(){
    
        jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
        auto eigentestJacobian=::grl::vrep::getJacobian(*VrepRobotArmDriverP_);

        /// The row/column major order is swapped between cisst and VREP!
        this->currentKinematicsStateP_->Jacobian.SetSize(eigentestJacobian.cols(),eigentestJacobian.rows());
        Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> > mckp2(this->currentKinematicsStateP_->Jacobian.Pointer(),this->currentKinematicsStateP_->Jacobian.cols(),this->currentKinematicsStateP_->Jacobian.rows());
        mckp2 = eigentestJacobian.cast<double>();
        
        
        //Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> > mf(eigentestJacobian,eigentestJacobian.cols(),eigentestJacobian.rows());
        //Eigen::MatrixXf eigenJacobian = mf;
        Eigen::MatrixXf eigenJacobian = eigentestJacobian;
        
        
        ///////////////////////////////////////////////////////////
        // Copy Joint Interval, the range of motion for each joint
        
        
        // lower limits
        auto & llim = std::get<vrep::VrepRobotArmDriver::JointLowerPositionLimit>(currentArmState_);
        std::vector<double> llimits(llim.begin(),llim.end());
        jointPositionLimitsVFP_->LowerLimits = vctDoubleVec(llimits.size(),&llimits[0]);
        
        // upper limits
        auto & ulim = std::get<vrep::VrepRobotArmDriver::JointUpperPositionLimit>(currentArmState_);
        std::vector<double> ulimits(ulim.begin(),ulim.end());
        jointPositionLimitsVFP_->UpperLimits = vctDoubleVec(ulimits.size(),&ulimits[0]);
        
        // current position
        auto & currentJointPos = std::get<vrep::VrepRobotArmDriver::JointPosition>(currentArmState_);
        std::vector<double> currentJointPosVec(currentJointPos.begin(),currentJointPos.end());
        vctDoubleVec vctDoubleVecCurrentJoints(currentJointPosVec.size(),&currentJointPosVec[0]);
        
        // update limits
        /// @todo does this leak memory when called every time around?
        UpdateJointPosLimitsVF(positionLimitsName,jointPositionLimitsVFP_->UpperLimits,jointPositionLimitsVFP_->LowerLimits,vctDoubleVecCurrentJoints);
        
        
        const auto& handleParams = VrepRobotArmDriverP_->getVrepHandleParams();
        Eigen::Affine3d currentEndEffectorPose =
        getObjectTransform(
                             std::get<vrep::VrepRobotArmDriver::RobotTipName>(handleParams)
                            ,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
                          );
        auto  currentEigenT = currentEndEffectorPose.translation();
        auto& currentCisstT = currentKinematicsStateP_->Frame.Translation();
        currentCisstT[0] = currentEigenT(0);
        currentCisstT[1] = currentEigenT(1);
        currentCisstT[2] = currentEigenT(2);
#ifndef IGNORE_ROTATION
        Eigen::Map<Eigen::Matrix<double,3,3,Eigen::ColMajor>> ccr(currentKinematicsStateP_->Frame.Rotation().Pointer());
        ccr = currentEndEffectorPose.rotation();
#endif // IGNORE_ROTATION
        /// @todo set rotation component of current position
        
        
        Eigen::Affine3d desiredEndEffectorPose =
        getObjectTransform(
                             std::get<vrep::VrepRobotArmDriver::RobotTargetName>(handleParams)
                            ,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
                          );
        auto  desiredEigenT = desiredEndEffectorPose.translation();
        auto& desiredCisstT = desiredKinematicsStateP_->Frame.Translation();
        desiredCisstT[0] = desiredEigenT(0);
        desiredCisstT[1] = desiredEigenT(1);
        desiredCisstT[2] = desiredEigenT(2);
#ifndef IGNORE_ROTATION
        Eigen::Map<Eigen::Matrix<double,3,3,Eigen::ColMajor>> dcr(desiredKinematicsStateP_->Frame.Rotation().Pointer());
        dcr = desiredEndEffectorPose.rotation();
#endif // IGNORE_ROTATION
        /// @todo set rotation component of desired position
        
        // for debugging, the translation between the current and desired position in cartesian coordinates
        auto inputDesired_dx = desiredCisstT - currentCisstT;
        
        vct3 dx_translation, dx_rotation;
        
        // Rotation part
        vctAxAnRot3 dxRot;
        vct3 dxRotVec;
        dxRot.FromNormalized((currentKinematicsStateP_->Frame.Inverse() * desiredKinematicsStateP_->Frame).Rotation());
        dxRotVec = dxRot.Axis() * dxRot.Angle();
        dx_rotation[0] = dxRotVec[0];
        dx_rotation[1] = dxRotVec[1];
        dx_rotation[2] = dxRotVec[2];
        //dx_rotation.SetAll(0.0);
        dx_rotation = currentKinematicsStateP_->Frame.Rotation() * dx_rotation;
        
        Eigen::AngleAxis<float> tipToTarget_cisstToEigen;
        
        Eigen::Matrix3f rotmat;
        double theta = std::sqrt(dx_rotation[0]*dx_rotation[0]+dx_rotation[1]*dx_rotation[1]+dx_rotation[2]*dx_rotation[2]);
        rotmat= Eigen::AngleAxisf(theta,Eigen::Vector3f(dx_rotation[0]/theta,dx_rotation[1]/theta,dx_rotation[2]/theta));
        
//        std::cout << "\ntiptotarget     \n" << tipToTarget.matrix() << "\n";
//        std::cout << "\ntiptotargetcisst\n" << rotmat.matrix() << "\n";
        
        
        //BOOST_LOG_TRIVIAL(trace) << "\n   test         desired dx: " << inputDesired_dx << " " << dx_rotation << "\noptimizer Calculated dx: " << optimizerCalculated_dx;
        SetKinematics(*currentKinematicsStateP_);  // replaced by name of object
        // fill these out in the desiredKinematicsStateP_
        //RotationType RotationMember; // vcRot3
        //TranslationType TranslationMember; // vct3
    
        SetKinematics(*desiredKinematicsStateP_); // replaced by name of object
        // call setKinematics with the new kinematics
        // sawconstraintcontroller has kinematicsState
        // set the jacobian here
        
        //////////////////////
        /// @todo move code below here back under run_one updateKinematics() call
        
       /// @todo need to provide tick time in double seconds and get from vrep API call
       float simulationTimeStep = simGetSimulationTimeStep();
       UpdateOptimizer(simulationTimeStep);
       
       vctDoubleVec jointAngles_dt;
       auto returncode = Solve(jointAngles_dt);
       
       
       /// @todo check the return code, if it doesn't have a result, use the VREP version as a fallback and report an error.
       if(returncode != nmrConstraintOptimizer::NMR_OK) BOOST_THROW_EXCEPTION(std::runtime_error("VrepInverseKinematicsController: constrained optimization error, please investigate"));
       
       
       /// @todo: rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command?
        std::string str;
       // str = "";
       for (std::size_t i=0 ; i < jointHandles_.size() ; i++)
       {
          float currentAngle;
          auto ret = simGetJointPosition(jointHandles_[i],&currentAngle);
          BOOST_VERIFY(ret!=-1);
          float futureAngle = currentAngle + jointAngles_dt[i];
          //simSetJointTargetVelocity(jointHandles_[i],jointAngles_dt[i]/simulationTimeStep);
          //simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]);
          //simSetJointTargetPosition(jointHandles_[i],futureAngle);
          simSetJointPosition(jointHandles_[i],futureAngle);
                str+=boost::lexical_cast<std::string>(jointAngles_dt[i]);
                if (i<jointHandles_.size()-1)
                    str+=", ";
       }
        BOOST_LOG_TRIVIAL(trace) << "jointAngles_dt: "<< str;
        
        auto optimizerCalculated_dx = this->currentKinematicsStateP_->Jacobian * jointAngles_dt;
       
        BOOST_LOG_TRIVIAL(trace) << "\n            desired dx: " << inputDesired_dx << " " << dx_rotation << "\noptimizer Calculated dx: " << optimizerCalculated_dx;
    }