void getCols(std::set<int> &cols, MatrixBase<DerivedA> const &M,
MatrixBase<DerivedB> &Msub) {
if (static_cast<int>(cols.size()) == M.cols()) {
Msub = M;
return;
}
int i = 0;
for (std::set<int>::iterator iter = cols.begin(); iter != cols.end(); iter++)
Msub.col(i++) = M.col(*iter);
}
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
void getCols(std::set<int>& cols, MatrixBase<DerivedA> const& M,
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
MatrixBase<DerivedB>& Msub) {
if (static_cast<int>(cols.size()) == M.cols()) {
Msub = M;
return;
}
int i = 0;
for (std::set<int>::iterator iter = cols.begin(); iter != cols.end(); iter++)
Msub.col(i++) = M.col(*iter);
}
void dumpPointsMatrix(std::string filePath, const MatrixBase<Derived>& v)
{
std::fstream l(filePath.c_str());
if (!l.good())
{
GRSF_ERRORMSG("Could not open file: " << filePath << std::endl)
}
for (unsigned int i = 0; i < v.cols(); i++)
{
l << v.col(i).transpose().format(MyMatrixIOFormat::SpaceSep) << std::endl;
}
}
Matrix<Scalar, Dynamic, 1> dynamicsBiasTermTemp(const RigidBodyTree &model, KinematicsCache<Scalar> &cache, const MatrixBase<DerivedF> &f_ext_value) {
// temporary solution.
eigen_aligned_unordered_map<const RigidBody *, Matrix<Scalar, 6, 1> > f_ext;
if (f_ext_value.size() > 0) {
assert(f_ext_value.cols() == model.bodies.size());
for (DenseIndex i = 0; i < f_ext_value.cols(); i++) {
f_ext.insert({model.bodies[i].get(), f_ext_value.col(i)});
}
}
return model.dynamicsBiasTerm(cache, f_ext);
};
Matrix<Scalar, Dynamic, 1> dynamicsBiasTermTemp(
const RigidBodyTreed& model, KinematicsCache<Scalar>& cache,
const MatrixBase<DerivedF>& f_ext_value) {
// temporary solution.
typename RigidBodyTree<Scalar>::BodyToWrenchMap external_wrenches;
if (f_ext_value.size() > 0) {
DRAKE_ASSERT(f_ext_value.cols() == model.bodies.size());
for (Eigen::Index i = 0; i < f_ext_value.cols(); i++) {
external_wrenches.insert({model.bodies[i].get(), f_ext_value.col(i)});
}
}
return model.dynamicsBiasTerm(cache, external_wrenches);
}
bool checkPointsInOOBB(const MatrixBase<Derived>& points, OOBB oobb)
{
Matrix33 A_KI = oobb.m_q_KI.matrix();
A_KI.transposeInPlace();
Vector3 p;
bool allInside = true;
auto size = points.cols();
decltype(size) i = 0;
while (i < size && allInside)
{
p = A_KI * points.col(i);
allInside &= (p(0) >= oobb.m_minPoint(0) && p(0) <= oobb.m_maxPoint(0) && p(1) >= oobb.m_minPoint(1) &&
p(1) <= oobb.m_maxPoint(1) && p(2) >= oobb.m_minPoint(2) && p(2) <= oobb.m_maxPoint(2));
++i;
}
return allInside;
}
APPROXMVBB_EXPORT void samplePointsGrid(Matrix3Dyn & newPoints,
const MatrixBase<Derived> & points,
const unsigned int nPoints,
OOBB & oobb) {
if(nPoints >= points.cols() || nPoints < 2) {
ApproxMVBB_ERRORMSG("Wrong arguements!")
}
newPoints.resize(3,nPoints);
std::random_device rd;
std::mt19937 gen(rd());
std::uniform_int_distribution<unsigned int> dis(0, points.cols()-1);
//total points = bottomPoints=gridSize^2 + topPoints=gridSize^2
unsigned int gridSize = std::max( static_cast<unsigned int>( std::sqrt( static_cast<double>(nPoints) / 2.0 )) , 1U );
// Set z-Axis to longest dimension
//std::cout << oobb.m_minPoint.transpose() << std::endl;
oobb.setZAxisLongest();
unsigned int halfSampleSize = gridSize*gridSize;
std::vector< std::pair<unsigned int , PREC > > topPoints(halfSampleSize, std::pair<unsigned int,PREC>{} ); // grid of indices of the top points (indexed from 1 )
std::vector< std::pair<unsigned int , PREC > > bottomPoints(halfSampleSize, std::pair<unsigned int,PREC>{} ); // grid of indices of the bottom points (indexed from 1 )
using LongInt = long long int;
MyMatrix<LongInt>::Array2 idx; // Normalized P
//std::cout << oobb.extent() << std::endl;
//std::cout << oobb.m_minPoint.transpose() << std::endl;
Array2 dxdyInv = Array2(gridSize,gridSize) / oobb.extent().head<2>(); // in K Frame;
Vector3 K_p;
Matrix33 A_KI(oobb.m_q_KI.matrix().transpose());
// Register points in grid
auto size = points.cols();
for(unsigned int i=0; i<size; ++i) {
K_p = A_KI * points.col(i);
// get x index in grid
idx = ( (K_p - oobb.m_minPoint).head<2>().array() * dxdyInv ).template cast<LongInt>();
// map to grid
idx(0) = std::max( std::min( LongInt(gridSize-1), idx(0)), 0LL );
idx(1) = std::max( std::min( LongInt(gridSize-1), idx(1)), 0LL );
//std::cout << idx.transpose() << std::endl;
unsigned int pos = idx(0) + idx(1)*gridSize;
// Register points in grid
// if z axis of p is > topPoints[pos] -> set new top point at pos
// if z axis of p is < bottom[pos] -> set new bottom point at pos
if( topPoints[pos].first == 0) {
topPoints[pos].first = bottomPoints[pos].first = i+1;
topPoints[pos].second = bottomPoints[pos].second = K_p(2);
} else {
if( topPoints[pos].second < K_p(2) ) {
topPoints[pos].first = i+1;
topPoints[pos].second = K_p(2);
}
if( bottomPoints[pos].second > K_p(2) ) {
bottomPoints[pos].first = i+1;
bottomPoints[pos].second = K_p(2);
}
}
}
// Copy top and bottom points
unsigned int k=0;
// k does not overflow -> 2* halfSampleSize = 2*gridSize*gridSize <= nPoints;
for(unsigned int i=0; i<halfSampleSize; ++i) {
if( topPoints[i].first != 0 ) {
// comment in if you want the points top points of the grid
// Array3 a(i % gridSize,i/gridSize,oobb.m_maxPoint(2)-oobb.m_minPoint(2));
// a.head<2>()*=dxdyInv.inverse();
newPoints.col(k++) = points.col(topPoints[i].first-1); // A_KI.transpose()*(oobb.m_minPoint + a.matrix()).eval() ;
if(topPoints[i].first != bottomPoints[i].first) {
// comment in if you want the bottom points of the grid
// Array3 a(i % gridSize,i/gridSize,0);
// a.head<2>()*=dxdyInv.inverse();
newPoints.col(k++) = points.col(bottomPoints[i].first-1); // A_KI.transpose()*(oobb.m_minPoint + a.matrix()).eval() ;
}
}
}
// Add random points!
while( k < nPoints) {
newPoints.col(k++) = points.col( dis(gen) ); //= Vector3(0,0,0);//
}
}
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);
}
}
