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);
}
}
}
// 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)
}
// 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);
}
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;
}
}
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);
}
/// 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(),¤tJointPosVec[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],¤tAngle);
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;
}