// //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; }