// 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_simulationabouttostart)
{
// Simulation is about to start
ROS_INFO("[V-REP_DXL_ARM] Simulation is about to start.");
getJointHandles();
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{
std::cout << " ---- ---- ---- " << std::endl;
// A simulation iteration is about to take place
for (int i = 0; i < JOINT_COUNT; ++i)
{
simGetJointPosition(joint_handles[i], &msg.data[i]);
msg.data[i] *= RAD_TO_DEG;
msg.data[i] += 150;
std::cout << msg.data[i] << std::endl;
}
msg.data[JOINT_COUNT] = 150; // Grasp -> center position
pub.publish(msg);
}
if (message==sim_message_eventcallback_simulationended)
{
// Simulation just ended
ROS_INFO("[V-REP_DXL_ARM] Simulation just ended.");
}
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode, errorModeSaved); // restore previous settings
simLockInterface(0);
return (retVal);
}
void JointControlPluglet::velocityCallback(
const brics_actuator::JointVelocities msg) {
//ctl_state = Velocity;
//}
for (unsigned int i = 0; i < msg.velocities.size(); i++) {
if (!controlledJoint(msg.velocities[i].joint_uri)) {
continue;
}
int handle = simGetObjectHandle(msg.velocities[i].joint_uri.c_str());
if (handle > 0) {
if (std::fabs(msg.velocities[i].value) < 0.0000001) {
brics_actuator::JointPositions msg_single;
msg_single.poisonStamp = msg.poisonStamp;
brics_actuator::JointValue value = msg.velocities[i];
value.unit = "rad";
float dval = 0.0;
simGetJointPosition(handles[i], &dval);
value.value = dval;
msg_single.positions.push_back(value);
//positionCallback(msg_single);
simSetObjectIntParameter(handle, 2000, 1);
simSetObjectIntParameter(handle, 2001, 1);
simSetJointTargetPosition(handle, dval);
} else {
simSetObjectIntParameter(handle, 2000, 1);
simSetObjectIntParameter(handle, 2001, 0);
simSetObjectIntParameter(handle, 1000, 1);
simSetJointTargetVelocity(handle, static_cast<float>(msg.velocities[i].value));
}
}
}
}
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;
}
