void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
if(nlhs != 19 || nrhs != 1)
{
mexErrMsgIdAndTxt("Drake:testIKoptions:BadInputs","Usage [robot_address,Q,Qa,Qv,debug_mode, sequentialSeedFlag,majorFeasibilityTolerance,majorIterationsLimit,iterationsLimit,superbasicsLimit,majorOptimalityTolerance,additional_tSamples,fixInitialState,q0_lb,q0_ub,qd0_lb,qd0_ub,qdf_lb,qdf_ub] = testIKoptionsmex(ikoptions_ptr)");
}
IKoptions* ikoptions = (IKoptions*) getDrakeMexPointer(prhs[0]);
long long robot_address = reinterpret_cast<long long>(ikoptions->getRobotPtr());
int nq = ikoptions->getRobotPtr()->num_positions;
MatrixXd Q;
ikoptions->getQ(Q);
MatrixXd Qv;
ikoptions->getQv(Qv);
MatrixXd Qa;
ikoptions->getQa(Qa);
bool debug_mode = ikoptions->getDebug();
bool sequentialSeedFlag = ikoptions->getSequentialSeedFlag();
double majorFeasibilityTolerance = ikoptions->getMajorFeasibilityTolerance();
int majorIterationsLimit = ikoptions->getMajorIterationsLimit();
int iterationsLimit = ikoptions->getIterationsLimit();
int superbasicsLimit = ikoptions->getSuperbasicsLimit();
double majorOptimalityTolerance = ikoptions->getMajorOptimalityTolerance();
RowVectorXd t_samples;
ikoptions->getAdditionaltSamples(t_samples);
bool fixInitialState = ikoptions->getFixInitialState();
VectorXd q0_lb,q0_ub;
VectorXd qd0_lb,qd0_ub;
VectorXd qdf_lb,qdf_ub;
ikoptions->getq0(q0_lb,q0_ub);
ikoptions->getqd0(qd0_lb,qd0_ub);
ikoptions->getqdf(qdf_lb,qdf_ub);
plhs[0] = mxCreateDoubleScalar((double) robot_address);
plhs[1] = mxCreateDoubleMatrix(nq,nq,mxREAL);
memcpy(mxGetPr(plhs[1]),Q.data(),sizeof(double)*nq*nq);
plhs[2] = mxCreateDoubleMatrix(nq,nq,mxREAL);
memcpy(mxGetPr(plhs[2]),Qa.data(),sizeof(double)*nq*nq);
plhs[3] = mxCreateDoubleMatrix(nq,nq,mxREAL);
memcpy(mxGetPr(plhs[3]),Qv.data(),sizeof(double)*nq*nq);
plhs[4] = mxCreateLogicalScalar(debug_mode);
plhs[5] = mxCreateLogicalScalar(sequentialSeedFlag);
plhs[6] = mxCreateDoubleScalar(majorFeasibilityTolerance);
plhs[7] = mxCreateDoubleScalar((double) majorIterationsLimit);
plhs[8] = mxCreateDoubleScalar((double) iterationsLimit);
plhs[9] = mxCreateDoubleScalar((double) superbasicsLimit);
plhs[10] = mxCreateDoubleScalar(majorOptimalityTolerance);
if(t_samples.size()>0)
{
plhs[11] = mxCreateDoubleMatrix(1,static_cast<int>(t_samples.size()),mxREAL);
memcpy(mxGetPr(plhs[11]),t_samples.data(),sizeof(double)*t_samples.size());
}
else
{
plhs[11] = mxCreateDoubleMatrix(0,0,mxREAL);
}
plhs[12] = mxCreateLogicalScalar(fixInitialState);
plhs[13] = mxCreateDoubleMatrix(nq,1,mxREAL);
memcpy(mxGetPr(plhs[13]),q0_lb.data(),sizeof(double)*nq);
plhs[14] = mxCreateDoubleMatrix(nq,1,mxREAL);
memcpy(mxGetPr(plhs[14]),q0_ub.data(),sizeof(double)*nq);
plhs[15] = mxCreateDoubleMatrix(nq,1,mxREAL);
memcpy(mxGetPr(plhs[15]),qd0_lb.data(),sizeof(double)*nq);
plhs[16] = mxCreateDoubleMatrix(nq,1,mxREAL);
memcpy(mxGetPr(plhs[16]),qd0_ub.data(),sizeof(double)*nq);
plhs[17] = mxCreateDoubleMatrix(nq,1,mxREAL);
memcpy(mxGetPr(plhs[17]),qdf_lb.data(),sizeof(double)*nq);
plhs[18] = mxCreateDoubleMatrix(nq,1,mxREAL);
memcpy(mxGetPr(plhs[18]),qdf_ub.data(),sizeof(double)*nq);
}
void inverseKinBackend(
RigidBodyTree* model, const int nT,
const double* t, const MatrixBase<DerivedA>& q_seed,
const MatrixBase<DerivedB>& q_nom, const int num_constraints,
RigidBodyConstraint** const constraint_array,
const IKoptions& ikoptions, MatrixBase<DerivedC>* q_sol,
int* info, std::vector<std::string>* infeasible_constraint) {
// Validate some basic parameters of the input.
if (q_seed.rows() != model->number_of_positions() || q_seed.cols() != nT ||
q_nom.rows() != model->number_of_positions() || q_nom.cols() != nT) {
throw std::runtime_error(
"Drake::inverseKinBackend: q_seed and q_nom must be of size "
"nq x nT");
}
KinematicsCacheHelper<double> kin_helper(model->bodies);
// TODO(sam.creasey) I really don't like rebuilding the
// OptimizationProblem for every timestep, but it's not possible to
// enable/disable (or even remove) a constraint from an
// OptimizationProblem between calls to Solve() currently, so
// there's not actually another way.
for (int t_index = 0; t_index < nT; t_index++) {
OptimizationProblem prog;
SetIKSolverOptions(ikoptions, &prog);
DecisionVariableView vars =
prog.AddContinuousVariables(model->number_of_positions());
MatrixXd Q;
ikoptions.getQ(Q);
auto objective = std::make_shared<InverseKinObjective>(model, Q);
prog.AddCost(objective, {vars});
for (int i = 0; i < num_constraints; i++) {
RigidBodyConstraint* constraint = constraint_array[i];
const int constraint_category = constraint->getCategory();
if (constraint_category ==
RigidBodyConstraint::SingleTimeKinematicConstraintCategory) {
SingleTimeKinematicConstraint* stc =
static_cast<SingleTimeKinematicConstraint*>(constraint);
if (!stc->isTimeValid(&t[t_index])) { continue; }
auto wrapper = std::make_shared<SingleTimeKinematicConstraintWrapper>(
stc, &kin_helper);
prog.AddConstraint(wrapper, {vars});
} else if (constraint_category ==
RigidBodyConstraint::PostureConstraintCategory) {
PostureConstraint* pc = static_cast<PostureConstraint*>(constraint);
if (!pc->isTimeValid(&t[t_index])) { continue; }
VectorXd lb;
VectorXd ub;
pc->bounds(&t[t_index], lb, ub);
prog.AddBoundingBoxConstraint(lb, ub, {vars});
} else if (
constraint_category ==
RigidBodyConstraint::SingleTimeLinearPostureConstraintCategory) {
AddSingleTimeLinearPostureConstraint(
&t[t_index], constraint, model->number_of_positions(),
vars, &prog);
} else if (constraint_category ==
RigidBodyConstraint::QuasiStaticConstraintCategory) {
AddQuasiStaticConstraint(&t[t_index], constraint, &kin_helper,
vars, &prog);
} else if (constraint_category ==
RigidBodyConstraint::MultipleTimeKinematicConstraintCategory) {
throw std::runtime_error(
"MultipleTimeKinematicConstraint is not supported"
" in pointwise mode.");
} else if (
constraint_category ==
RigidBodyConstraint::MultipleTimeLinearPostureConstraintCategory) {
throw std::runtime_error(
"MultipleTimeLinearPostureConstraint is not supported"
" in pointwise mode.");
}
}
// Add a bounding box constraint from the model.
prog.AddBoundingBoxConstraint(
model->joint_limit_min, model->joint_limit_max,
{vars});
// TODO(sam.creasey) would this be faster if we stored the view
// instead of copying into a VectorXd?
objective->set_q_nom(q_nom.col(t_index));
if (!ikoptions.getSequentialSeedFlag() || (t_index == 0)) {
prog.SetInitialGuess(vars, q_seed.col(t_index));
} else {
prog.SetInitialGuess(vars, q_sol->col(t_index - 1));
}
SolutionResult result = prog.Solve();
prog.PrintSolution();
q_sol->col(t_index) = vars.value();
info[t_index] = GetIKSolverInfo(prog, result);
}
}
