void mexFunction(int nlhs, mxArray *plhs[], /* Output variables */
int nrhs, const mxArray *prhs[]) /* Input variables */
{
#define ccr(ai,bi) (ccr[ai+Nss[0]*bi])
#define cci(ai,bi) (cci[ai+Nss[0]*bi])
#define kk1(ai,bi) (kk1[ai+Nss[0]*bi])
#define kk2(ai,bi) (kk2[ai+Nss[0]*bi])
#define kb(loc1,loc2,ai,bi) kb[loc1+NB1*(loc2+NB2*(ai+Nss[0]*bi))]
#define avgdx(loc1,loc2,ai,bi) avgdx[loc1+NB1*(loc2+NB2*(ai+Nss[0]*bi))]
#define avgdy(loc1,loc2,ai,bi) avgdy[loc1+NB1*(loc2+NB2*(ai+Nss[0]*bi))]
size_t ai, bi;
int NB1, NB2, loc1, loc2, di = 0;
double *kk1, *kk2, *ccr, *cci, *kb, *avgdx, *avgdy;
double EXT, num_dir, da, dr, r, agl, R_low;
double temp_energy;
ccr = mxGetPr(prhs[0]);
cci = mxGetPi(prhs[0]);
kk1 = mxGetPr(prhs[1]);
kk2 = mxGetPr(prhs[2]);
EXT = mxGetScalar(prhs[3]);
num_dir = mxGetScalar(prhs[4]);
da = mxGetScalar(prhs[5]);
dr = mxGetScalar(prhs[6]);
NB1 = mxGetScalar(prhs[7]);
NB2 = mxGetScalar(prhs[8]);
R_low = mxGetScalar(prhs[9]);
const mwSize *Nss = mxGetDimensions(prhs[0]);
kb = mxGetPr(prhs[10]);
avgdx = mxGetPr(prhs[11]);
avgdy = mxGetPr(prhs[12]);
nrhs = 13;
nlhs = 0;
for (ai=0;ai<Nss[0];ai++) {
for (bi=0;bi<Nss[1];bi++) {
if (kk1(ai,bi)<EXT) {
r = sqrt(kk1(ai,bi)*kk1(ai,bi)+kk2(ai,bi)*kk2(ai,bi));
if (kk1(ai,bi)>=0) {
agl = fmod(acos(kk2(ai,bi)/r),num_dir);
}
else
agl = fmod(3.1415926-acos(kk2(ai,bi)/r),num_dir);
loc1 = round((r-R_low)/dr);
loc2 = round(agl/da);
temp_energy = ccr(ai,bi)*ccr(ai,bi) + cci(ai,bi)*cci(ai,bi);
kb(loc1,loc2,ai,bi) = kb(loc1,loc2,ai,bi) + temp_energy;
avgdx(loc1,loc2,ai,bi) = avgdx(loc1,loc2,ai,bi) + r * cos(agl) * temp_energy;
avgdy(loc1,loc2,ai,bi) = avgdy(loc1,loc2,ai,bi) + r * sin(agl) * temp_energy;
/* cannot use kk since those are symmetric */
}
}
}
return;
}
void CCArcticManager::parseModule(CCArcticModule* arcticModule, CCArcticFrameModule* arcticFrameModule, CCAFCClip* afcClip, int index, int offsetX, int offsetY) {
CCAFCClipData& afcClipData = afcClip->getData();
// set type
afcClip->setType(AFC_CLIP_IMAGE);
// set index
afcClip->setIndex(index);
// image index
afcClipData.i.imageIndex = arcticModule->imageIndex;
// clip pos
// ASprite y axis is reversed with opengl y axis, and origin is top left corner
afcClipData.clipPos = ccpt(resolve(arcticFrameModule->x + arcticModule->w / 2 + offsetX),
resolve(-arcticFrameModule->y - arcticModule->h / 2 - offsetY));
// save image rect
afcClipData.i.rect = ccr(resolve(arcticModule->x), resolve(arcticModule->y), resolve(arcticModule->w), resolve(arcticModule->h));
// set flip flag
afcClipData.i.flipX = (arcticFrameModule->flags & AS_FLIP_X) != 0;
if((arcticFrameModule->flags & AS_FLIP_Y) != 0) {
afcClipData.i.flipX = !afcClipData.i.flipX;
afcClipData.i.rotation = 180;
}
}
bool Knowledge::IsInsideGoalShape(Vector2D pos, double goalLeftX, double goalRadius, double goalCcOffset)
{
double x = pos.x - goalLeftX;
Vector2D ccl(goalLeftX, goalCcOffset / 2), ccr(goalLeftX, -goalCcOffset / 2);
return (pos.dist(ccl) <= goalRadius || pos.dist(ccr) <= goalRadius ||
(x >= 0 && x <= goalRadius && fabs(pos.y) <= goalCcOffset / 2));
}
KDbool CScrMainMenu::init ( KDvoid )
{
// 레이어를 초기화 한다.
if ( !CCLayer::initWithVisibleViewport ( ) )
{
return KD_FALSE;
}
// 현재 레이어 사이즈를 가져온다.
CCSize tLyrSize = this->getContentSize ( );
CCSize tSize = ccsz;
// 디버그용 배경
// this->addChild ( CCLayerColor::create ( ccc4 ( 255, 0, 0, 128 ), tLyrSize ) );
// 우상단 종료 버튼
CCMenuItemImage* pClose = CCMenuItemImage::create
(
"Images/CloseNormal.png",
"Images/CloseSelected.png",
this, menu_selector ( CScrMainMenu::onExit )
);
this->addChild ( CCMenu::createWithItem ( pClose ), 1 );
tSize = pClose->getContentSize ( );
pClose->setPosition ( this, kCCAlignmentTopRight, ccp ( 20, 20 ) );
// pClose->setPosition ( ccp ( tLyrSize.cx - tSize.cx / 2 - 20, tLyrSize.cy - tSize.cx / 2 - 20 ) );
// 타이틀 레이블 생성
CCLabelTTF* pTitle = CCLabelTTF::create ( GSTAT->getText ( eTxtTitle ), "Font/NanumGothicBold.ttf", 40 );
this->addChild ( pTitle );
pTitle->setColor ( ccYELLOW );
pTitle->setPositionWithParent ( kCCAlignmentTop, ccp ( 0, 50 ) );
// 설명 Label 생성
CCLabelTTF* pDesc = CCLabelTTF::create( "Press the airplane !!!!", "Font/NanumGothicBold.ttf", 25 );
this->addChild( pDesc );
pDesc->setPosition ( ccpMid ( tLyrSize ) );
// 설명 Label이 계속해서 깜빡거리도록 설정
pDesc->runAction ( CCRepeatForever::create ( CCBlink::create ( 1.0f, 1 ) ) );
// 비행기 묶음 텍스쳐 생성
CCTexture2D* pTexture = CCTextureCache::sharedTextureCache ( )->addImage ( "Images/galagasheet.png" );
// 하단에 비행기 생성
CCSprite* pAirPlane = CCSprite::createWithTexture ( pTexture, ccr ( 184, 55, 15, 17 ) );
this->addChild ( pAirPlane );
pAirPlane->setScale ( 2 );
pAirPlane->setPositionWithParent ( kCCAlignmentBottom, ccp ( 0, 50 ) );
m_pAirPlane = pAirPlane;
// 레이어 터치 모드 켜기
this->setTouchEnabled ( KD_TRUE );
return KD_TRUE;
}
KDvoid TestBox2D::addNewSpriteWithCoords ( const CCPoint& tPosition )
{
// Define the dynamic body.
//Set up a 1m squared box in the physics world
b2BodyDef tBodyDef;
tBodyDef.type = b2_dynamicBody;
tBodyDef.position.Set ( tPosition.x / PTM_RATIO, tPosition.y / PTM_RATIO );
//tBodyDef.userData = sprite;
b2Body* pBody = m_pWorld->CreateBody ( &tBodyDef );
// Define another box shape for our dynamic body.
b2PolygonShape tDynamicBox;
tDynamicBox.SetAsBox ( 0.5f, 0.5f );
// Define the dynamic body fixture.
b2FixtureDef tFixtureDef;
tFixtureDef.shape = &tDynamicBox;
tFixtureDef.density = 1.0f;
tFixtureDef.friction = 0.3f;
pBody->CreateFixture ( &tFixtureDef );
CCNode* pParent = this->getChildByTag ( kTagParentNode );
// We have a 64x64 sprite sheet with 4 different 32x32 images. The following code is
// just randomly picking one of the images
KDint nOffsetX = ( CCRANDOM_0_1 ( ) > 0.5f ? 0 : 1 );
KDint nOffsetY = ( CCRANDOM_0_1 ( ) > 0.5f ? 0 : 1 );
CCPhysicsSprite* pSprite = CCPhysicsSprite::createWithTexture ( m_pSpriteTexture, ccr ( 32 * nOffsetX, 32 * nOffsetY, 32, 32 ) );
pParent->addChild ( pSprite );
pSprite->setB2Body ( pBody );
pSprite->setPTMRatio ( PTM_RATIO );
pSprite->setPosition ( tPosition );
}
bool CCMWManager::parseClip(CCMWFileData* animationFileData, int clipAdditionalDataIndex, CCMWClipType type, float clipPosX, float clipPosY, CCAFCClip* afcClip) {
CCAFCClipData& afcClipData = afcClip->getData();
// process based on clip type
if(type % 2 == 0) {
afcClip->setType(AFC_CLIP_IMAGE);
// image index
afcClipData.i.imageIndex = type / 8;
// flip flag
switch(type & 0x7) {
case ClipType_ImageFlipX:
afcClipData.i.flipX = true;
break;
case ClipType_ImageFlipY:
afcClipData.i.flipX = true;
afcClipData.i.rotation = 180;
break;
case ClipType_ImageFlipXY:
afcClipData.i.rotation = 180;
break;
}
// image rect in atlas
afcClipData.i.rect = ccr(resolve(animationFileData->m_imageClipPool[clipAdditionalDataIndex * 4]),
resolve(animationFileData->m_imageClipPool[clipAdditionalDataIndex * 4 + 1]),
resolve(animationFileData->m_imageClipPool[clipAdditionalDataIndex * 4 + 2]),
resolve(animationFileData->m_imageClipPool[clipAdditionalDataIndex * 4 + 3]));
// in motion welder coordinate system, upper y axis is negative
// we need to reverse it
// don't resolve y again because clippos and image rect are all resolved before
afcClipData.clipPos = ccpt(clipPosX + afcClipData.i.rect.width / 2,
-clipPosY - afcClipData.i.rect.height / 2);
} else if(type == ClipType_CollisionRect) {
afcClip->setType(AFC_CLIP_COLLISION_RECT);
afcClipData.cr.size = ccsz(resolve(animationFileData->m_positionerRectangleClipPool[clipAdditionalDataIndex * 2]),
resolve(animationFileData->m_positionerRectangleClipPool[clipAdditionalDataIndex * 2 + 1]));
// in motion welder coordinate system, upper y axis is negative
// we need to reverse it
// don't resolve y again because clippos and image rect are all resolved before
afcClipData.clipPos = ccpt(clipPosX + afcClipData.cr.size.width / 2,
-clipPosY - afcClipData.cr.size.height / 2);
} else if(type == ClipType_Line) {
afcClip->setType(AFC_CLIP_LINE);
afcClipData.l.endPoint = ccpt(resolve(animationFileData->m_lineClipPool[clipAdditionalDataIndex * 2]),
resolve(animationFileData->m_lineClipPool[clipAdditionalDataIndex * 2 + 1]));
afcClipData.l.color = animationFileData->m_lineClipPool[clipAdditionalDataIndex * 2 + 2];
afcClipData.clipPos = ccpt(clipPosX, -clipPosY);
} else if(type == ClipType_Rect || type == ClipType_Rect_Filled) {
afcClip->setType(AFC_CLIP_RECT);
afcClipData.r.size = ccsz(resolve(animationFileData->m_rectangleClipPool[clipAdditionalDataIndex * 2]),
resolve(animationFileData->m_rectangleClipPool[clipAdditionalDataIndex * 2 + 1]));
afcClipData.r.color = animationFileData->m_rectangleClipPool[clipAdditionalDataIndex * 2 + 2];
// in motion welder coordinate system, upper y axis is negative
// we need to reverse it
// don't resolve y again because clippos and image rect are all resolved before
afcClipData.clipPos = ccpt(clipPosX + afcClipData.r.size.width / 2,
-clipPosY - afcClipData.r.size.height / 2);
} else if(type == ClipType_RoundRect || type == ClipType_RoundRect_Filled) {
afcClip->setType(AFC_CLIP_ROUNDRECT);
afcClipData.rr.size = ccsz(resolve(animationFileData->m_roundedRectangleClipPool[clipAdditionalDataIndex * 2]),
resolve(animationFileData->m_roundedRectangleClipPool[clipAdditionalDataIndex * 2 + 1]));
afcClipData.rr.arcWidth = resolve(animationFileData->m_roundedRectangleClipPool[clipAdditionalDataIndex * 2 + 2]);
afcClipData.rr.arcHeight = resolve(animationFileData->m_roundedRectangleClipPool[clipAdditionalDataIndex * 2 + 3]);
afcClipData.rr.color = animationFileData->m_roundedRectangleClipPool[clipAdditionalDataIndex * 2 + 4];
// in motion welder coordinate system, upper y axis is negative
// we need to reverse it
// don't resolve y again because clippos and image rect are all resolved before
afcClipData.clipPos = ccpt(clipPosX + afcClipData.rr.size.width / 2,
-clipPosY - afcClipData.rr.size.height / 2);
} else if(type == ClipType_Ecllipse || type == ClipType_Ecllipse_Filled) {
afcClip->setType(AFC_CLIP_ELLIPSE);
afcClipData.e.size = ccsz(resolve(animationFileData->m_ellipseClipPool[clipAdditionalDataIndex * 2]),
resolve(animationFileData->m_ellipseClipPool[clipAdditionalDataIndex * 2 + 1]));
afcClipData.e.startAngle = animationFileData->m_ellipseClipPool[clipAdditionalDataIndex * 2 + 2];
afcClipData.e.endAngle = animationFileData->m_ellipseClipPool[clipAdditionalDataIndex * 2 + 3];
afcClipData.e.color = animationFileData->m_ellipseClipPool[clipAdditionalDataIndex * 2 + 4];
// in motion welder coordinate system, upper y axis is negative
// we need to reverse it
// don't resolve y again because clippos and image rect are all resolved before
afcClipData.clipPos = ccpt(clipPosX + afcClipData.e.size.width / 2,
-clipPosY - afcClipData.e.size.height / 2);
} else {
return false;
}
return true;
}
void CCAuroraManager::parseModule(CCAuroraModule* auroraModule, CCAuroraFrameModule* auroraFrameModule, CCAFCClip* afcClip, int index, int offsetX, int offsetY) {
CCAFCClipData& afcClipData = afcClip->getData();
// set index
afcClip->setIndex(index);
// set module info
switch(auroraModule->type) {
case BSM_IMAGE:
// set type
afcClip->setType(AFC_CLIP_IMAGE);
// image index
afcClipData.i.imageIndex = auroraModule->imageIndex;
// clip pos
// BSprite y axis is reversed with opengl y axis, and origin is top left corner
afcClipData.clipPos = ccpt(resolve(auroraFrameModule->x + auroraModule->w / 2 + offsetX),
resolve(-auroraFrameModule->y - auroraModule->h / 2 - offsetY));
// save image rect
afcClipData.i.rect = ccr(resolve(auroraModule->x), resolve(auroraModule->y),
resolve(auroraModule->w), resolve(auroraModule->h));
// set flip flag
afcClipData.i.flipX = (auroraFrameModule->flags & BS_FLIP_X) != 0;
if((auroraFrameModule->flags & BS_FLIP_Y) != 0) {
afcClipData.i.flipX = !afcClipData.i.flipX;
afcClipData.i.rotation = 180;
}
break;
case BSM_RECT:
// set type
afcClip->setType(AFC_CLIP_RECT);
// clip pos
// BSprite y axis is reversed with opengl y axis, and origin is top left corner
afcClipData.clipPos = ccpt(resolve(auroraFrameModule->x + auroraModule->w / 2 + offsetX),
resolve(-auroraFrameModule->y - auroraModule->h / 2 - offsetY));
// set rect size
afcClipData.r.size.width = resolve(auroraModule->w);
afcClipData.r.size.height = resolve(auroraModule->h);
break;
case BSM_FILL_RECT:
// set type
afcClip->setType(AFC_CLIP_RECT);
// clip pos
// BSprite y axis is reversed with opengl y axis, and origin is top left corner
afcClipData.clipPos = ccpt(resolve(auroraFrameModule->x + auroraModule->w / 2 + offsetX),
resolve(-auroraModule->h / 2 - auroraFrameModule->y - offsetY));
// set rect size
afcClipData.r.size.width = resolve(auroraModule->w);
afcClipData.r.size.height = resolve(auroraModule->h);
// set color
afcClipData.r.color = auroraModule->color;
break;
}
}
/// 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;
}
