//
//double Exhume::TwoSpace::SubProcess(){
//return(AmplitudeSq*TwoParticleLIPS*Amp2CrossSection);
//define overall factor in font of Msq + DLIPS factor
//}
double Exhume::TwoSpace::WeightFunc(const double& _CosTheta){
//arbitrary weighting at low mass
//std::cout<<_CosTheta<<std::endl;
CosTheta=_CosTheta;
//std::cout<<CosTheta<<std::endl;
Phi=PhiMax;//arbitrary angle but amp might use it but shouldn'
//double wgt_hold=0.0;
//for(int ii=0;ii<100;ii++){
//double _SqrtsHat = (MassAtThetaScanHigh - MassAtThetaScanLow)*double(ii)/10
// + MassAtThetaScanLow;
//double _SqrtsHat=sqrt(4*PartonMass*PartonMass +
// MassAtThetaScan*MassAtThetaScan);
double _SqrtsHat;
if(PartonMass>2.0){
_SqrtsHat=2*PartonMass+0.001;
}
else{
_SqrtsHat=sqrt(4*PartonMass*PartonMass + 10.0);
}
SetKinematics(_SqrtsHat,0.0,0.0,0.0,0.0,0.0);
//std::cout<<"here"<<std::endl;
LIPS2Amp();//
//std::cout<<"here"<<std::endl;
double _wgt = SubProcess();//AmplitudeSq();
//if(_wgt > wgt_hold){
//wgt_hold=wgt;
//}
if(_wgt>MaximumSubProcessValue){
MaximumSubProcessCosTheta=_CosTheta;
MaximumSubProcessValue=_wgt;
std::cout<< MaximumSubProcessCosTheta<<std::endl;
std::cout<< MaximumSubProcessValue<<std::endl;
}
//std::cout<<_wgt<<"\t"<<CosTheta<<std::endl;
return(_wgt);
}
void MT_SteeredRobot::Donut(double spd, double radius)
{
SetKinematics(spd, spd/radius);
}
void MT_SteeredRobot::SetSpeedOmega(double inspd, double inomega)
{
SetKinematics(inspd, inomega);
}
void MT_SteeredRobot::SetOmega(double inomega)
{
SetKinematics(speed, inomega);
}
void MT_SteeredRobot::SetSpeed(double inspd)
{
SetKinematics(inspd, omega);
}
/// 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;
}
