IMP_Eigen::MatrixXd
GaussianProcessInterpolation::get_posterior_covariance_hessian(Floats x) const {
const_cast<GaussianProcessInterpolation *>(this)->update_flags_covariance();
// get how many and which particles are optimized
unsigned mnum = get_number_of_m_particles();
std::vector<bool> mopt;
unsigned mnum_opt = 0;
for (unsigned i = 0; i < mnum; i++) {
mopt.push_back(get_m_particle_is_optimized(i));
if (mopt.back()) mnum_opt++;
}
unsigned Onum = get_number_of_Omega_particles();
std::vector<bool> Oopt;
unsigned Onum_opt = 0;
for (unsigned i = 0; i < Onum; i++) {
Oopt.push_back(get_Omega_particle_is_optimized(i));
if (Oopt.back()) Onum_opt++;
}
// total number of optimized particles
unsigned num_opt = mnum_opt + Onum_opt;
// whether sigma is optimized
unsigned sigma_opt = Oopt[0] ? 1 : 0;
// cov_opt: number of optimized covariance particles without counting sigma
unsigned cov_opt = Onum_opt - sigma_opt;
// init matrix and fill with zeros at mean particle's indices
// dprior_cov(q,q)/(dsigma d.) is also zero
IMP_Eigen::MatrixXd ret(IMP_Eigen::MatrixXd::Zero(num_opt, num_opt));
// build vector of dcov(q,q)/dp1dp2 with p1 and p2 covariance particles
FloatsList xv;
xv.push_back(x);
FloatsList tmplist;
for (unsigned pa = 0; pa < Onum; ++pa) {
if (!Oopt[pa]) continue; // skip not optimized particles
if (pa == 0) continue; // skip sigma even when it is optimized
Floats tmp;
for (unsigned pb = pa; pb < Onum; ++pb) {
if (!Oopt[pb]) continue; // skip not optimized particles
// sigma has already been skipped
tmp.push_back(covariance_function_->get_second_derivative_matrix(
pa - 1, pb - 1, xv)(0, 0));
}
tmplist.push_back(tmp);
}
for (unsigned pa_opt = 0; pa_opt < cov_opt; pa_opt++)
for (unsigned pb_opt = pa_opt; pb_opt < cov_opt; pb_opt++)
ret.bottomRightCorner(cov_opt, cov_opt)(pa_opt, pb_opt) =
tmplist[pa_opt][pb_opt - pa_opt];
// compute and store w(q) derivatives (skip mean particles)
IMP_Eigen::MatrixXd dwqdp(M_, Onum_opt);
for (unsigned i = 0, j = 0; i < Onum; i++)
if (Oopt[i]) dwqdp.col(j++) = get_wx_vector_derivative(x, i + mnum);
// add d2cov/(dw(q)_k * dw(q)_l) * dw(q)^k/dp_i * dw(q)^l/dp_j
ret.bottomRightCorner(cov_opt, cov_opt).noalias() +=
dwqdp.rightCols(cov_opt).transpose() * get_d2cov_dwq_dwq() *
dwqdp.rightCols(cov_opt);
// compute and store Omega derivatives (skip mean particles)
std::vector<IMP_Eigen::MatrixXd> dOmdp;
for (unsigned i = 0; i < Onum; i++) {
if (Oopt[i]) dOmdp.push_back(get_Omega_derivative(i));
}
// add d2cov/(dOm_kl * dOm_mn) * dOm^kl/dp_i * dOm^mn/dp_j
std::vector<std::vector<IMP_Eigen::MatrixXd> > d2covdo;
for (unsigned m = 0; m < M_; m++) {
std::vector<IMP_Eigen::MatrixXd> tmp;
for (unsigned n = 0; n < M_; n++) tmp.push_back(get_d2cov_dOm_dOm(x, m, n));
d2covdo.push_back(tmp);
}
for (unsigned i = 0; i < Onum_opt; i++) {
IMP_Eigen::MatrixXd tmp(M_, M_);
for (unsigned m = 0; m < M_; ++m)
for (unsigned n = 0; n < M_; ++n)
tmp(m, n) = (d2covdo[m][n].transpose() * dOmdp[i]).trace();
for (unsigned j = i; j < Onum_opt; j++)
ret.bottomRightCorner(Onum_opt, Onum_opt)(i, j) +=
(dOmdp[j].transpose() * tmp).trace();
}
for (unsigned i = 0; i < d2covdo.size(); i++) d2covdo[i].clear();
d2covdo.clear();
// compute cross-term
std::vector<IMP_Eigen::MatrixXd> d2covdwdo;
for (unsigned k = 0; k < M_; k++)
d2covdwdo.push_back(get_d2cov_dwq_dOm(x, k));
// compute derivative contribution into temporary
IMP_Eigen::MatrixXd tmpH(Onum_opt, Onum_opt);
for (unsigned i = 0; i < Onum_opt; i++) {
IMP_Eigen::VectorXd tmp(M_);
for (unsigned k = 0; k < M_; k++)
tmp(k) = (d2covdwdo[k].transpose() * dOmdp[i]).trace();
for (unsigned j = 0; j < Onum_opt; j++)
tmpH(i, j) = dwqdp.col(j).transpose() * tmp;
}
ret.bottomRightCorner(Onum_opt, Onum_opt) += tmpH + tmpH.transpose();
// deallocate unused stuff
// tmpH.resize(0,0);
d2covdwdo.clear();
dOmdp.clear();
// dwqdp.resize(0,0);
// dcov/dw_k * d2w^k/(dp_i dp_j)
IMP_Eigen::VectorXd dcwq(get_dcov_dwq(x));
for (unsigned i = 0; i < cov_opt; i++)
for (unsigned j = i; j < cov_opt; j++)
ret.bottomRightCorner(cov_opt, cov_opt)(i, j) +=
dcwq.transpose() *
get_wx_vector_second_derivative(x, mnum + 1 + i, mnum + 1 + j);
// dcwq.resize(0,0);
// dcov/dOm_kl * d2Om^kl/(dp_i dp_j), zero when p_i or p_j is sigma or mean
IMP_Eigen::MatrixXd dcOm(get_dcov_dOm(x));
for (unsigned i = 0; i < cov_opt; i++)
for (unsigned j = i; j < cov_opt; j++)
ret.bottomRightCorner(cov_opt, cov_opt)(i, j) +=
(dcOm.transpose() * get_Omega_second_derivative(i + 1, j + 1))
.trace();
// dcOm.resize(0,0);
// return as symmetric matrix
for (unsigned i = mnum_opt; i < num_opt; ++i)
for (unsigned j = mnum_opt + 1; j < num_opt; ++j) ret(j, i) = ret(i, j);
return ret;
}
RobotInterface::Status Test_IK_QP::RobotInit(){
// resize the global variables
mTargetVelocity.Resize(IK_CONSTRAINTS);
mInitialJointPos.Resize(KUKA_DOF);
mJointPos.Resize(KUKA_DOF);
mJointKinematics.Resize(KUKA_DOF);
mJointDesPos.Resize(KUKA_DOF);
mJointTargetPos.Resize(KUKA_DOF);
mJointDesVel.Resize(KUKA_DOF);
mJointDesResidualVel.Resize(KUKA_DOF);
mJointTorque.Resize(KUKA_DOF);
lJointWeight.Resize(KUKA_DOF);
lPos.Resize(3);
lJoints.Resize(KUKA_DOF);
lTargetPos.Resize(3);
mJointPosAll.Resize(KUKA_DOF+FINGER_DOF);
mJointTorqueAll.Resize(KUKA_DOF+FINGER_DOF);
// for inverse kinematics
mJointVelLimitsUp.Resize(KUKA_DOF);
mJointVelLimitsDn.Resize(KUKA_DOF);
mJointVelocityLimitsDT.Resize(KUKA_DOF);
// initialize sensor group
mSensorsGroup.SetSensorsList(mRobot->GetSensors());
mActuatorsGroup.SetActuatorsList(mRobot->GetActuators());
mSensorsGroup.ReadSensors();
mEndEffectorId = mRobot->GetLinksCount()-1;
mKinematicChain.SetRobot(mRobot);
mKinematicChain.Create(0,0,mEndEffectorId);
// kinematic chain for virtual robot
mSKinematicChain = new sKinematics(KUKA_DOF, _dt);
//Constraints for the joints angles
mThetaMinus.Resize(KUKA_DOF);
mThetaPlus.Resize(KUKA_DOF);
//constraint for the joint velocities
Vector tmpMaxVel(KUKA_DOF);
double SafetyRange = 0.8;
mThetaMinus[0] = SafetyRange* DEG2RAD( -85.); mThetaPlus[0] = SafetyRange* DEG2RAD(85.); tmpMaxVel[0] = SafetyRange* DEG2RAD(110.0);
mThetaMinus[1] = SafetyRange* DEG2RAD( -90.); mThetaPlus[1] = SafetyRange* DEG2RAD(90.); tmpMaxVel[1] = SafetyRange*DEG2RAD(110.0);
mThetaMinus[2] = SafetyRange* DEG2RAD( -100.); mThetaPlus[2] = SafetyRange* DEG2RAD(100.); tmpMaxVel[2] = SafetyRange*DEG2RAD(128.0);
mThetaMinus[3] = SafetyRange* DEG2RAD( -110.); mThetaPlus[3] = SafetyRange* DEG2RAD(110.); tmpMaxVel[3] = SafetyRange*DEG2RAD(128.0);
mThetaMinus[4] = SafetyRange* DEG2RAD( -140.); mThetaPlus[4] = SafetyRange* DEG2RAD(140.); tmpMaxVel[4] = SafetyRange*DEG2RAD(204.0);
mThetaMinus[5] = SafetyRange* DEG2RAD( -90.); mThetaPlus[5] = SafetyRange* DEG2RAD(90.); tmpMaxVel[5] = SafetyRange*DEG2RAD(184.0);
mThetaMinus[6] = SafetyRange* DEG2RAD( -120.); mThetaPlus[6] = SafetyRange* DEG2RAD(120.); tmpMaxVel[6] = SafetyRange*DEG2RAD(184.0);
//initial joints position of the robot
Vector tmpInitPos(7);
tmpInitPos(0) = 0.2460;
tmpInitPos(1) = -1.4137;
tmpInitPos(2) = -1.0231;
tmpInitPos(3) = -1.7218;
tmpInitPos(4) = -0.2202;
tmpInitPos(5) = 0.5573;
tmpInitPos(6) = -0.2399;
//sets the DH parameters
mSKinematicChain->setDH(0, 0.0, 0.34, M_PI_2, 0, 1, mThetaMinus[0], mThetaPlus[0], tmpMaxVel[0]);
mSKinematicChain->setDH(1, 0.0, 0.00,-M_PI_2, 0, 1, mThetaMinus[1], mThetaPlus[1], tmpMaxVel[1]);
mSKinematicChain->setDH(2, 0.0, 0.40,-M_PI_2, 0, 1, mThetaMinus[2], mThetaPlus[2], tmpMaxVel[2]);
mSKinematicChain->setDH(3, 0.0, 0.00, M_PI_2, 0, 1, mThetaMinus[3], mThetaPlus[3], tmpMaxVel[3]);
mSKinematicChain->setDH(4, 0.0, 0.40, M_PI_2, 0, 1, mThetaMinus[4], mThetaPlus[4], tmpMaxVel[4]);
mSKinematicChain->setDH(5, 0.0, 0.00,-M_PI_2, 0 , 1, mThetaMinus[5], mThetaPlus[5], tmpMaxVel[5]);
mSKinematicChain->setDH(6, -0.05, 0.2290, 0.0, 0, 1, mThetaMinus[6],mThetaPlus[6], tmpMaxVel[6]);
mSKinematicChain->readyForKinematics();
//For the kinematic chain
double T0[4][4];
for(int i=0; i<4; i++)
for(int j=0; j<4; j++)
T0[i][j] = 0.0;
T0[0][0] = 1;
T0[1][1] = 1;
T0[2][2] = 1;
T0[3][3] = 1;
mSKinematicChain->setT0(T0);
// ready for kinematics
mSKinematicChain->readyForKinematics();
mSKinematicChain->setJoints(tmpInitPos.Array());
mJointKinematics.Set(tmpInitPos);
// Sets the number of DOF of the joints and of the end effector (here 6)
mSolver.setDimensions(KUKA_DOF, 6);
//Sets the constraints for the angles and the angles velocities
mSolver.setConstraints(mThetaMinus, mThetaPlus, tmpMaxVel*(-1), tmpMaxVel);
//Sets the weights for the inertia matrix H
lJointWeight(0) = 1.0;
lJointWeight(1) = 1.0;
lJointWeight(2) = 1.0;
lJointWeight(3) = 1.0;
lJointWeight(4) = 0.1;
lJointWeight(5) = 0.1;
lJointWeight(6) = 0.1;
//
Matrix tmpH(KUKA_DOF,KUKA_DOF);
for(unsigned int i(0); i<KUKA_DOF;++i){
tmpH(i,i) = lJointWeight[i];
}
//sets the inertia matrix
mSolver.setH(tmpH);
//sets the timestep
mSolver.setDeltaT(_dt);
//MuP is the hyperparameter that tunes the avoidance of the joints limits
mSolver.setMuP(25);
//Gamma sets the size of the steps for the convergence (if gamma is larger than 200 it will be unstable for the KUKA)
mSolver.setGamma(50);
//variables used for the trajectory
mTIME = 0;
mStep = 0;
AddConsoleCommand("test");
mPlanner =NONE;
return STATUS_OK;
}
