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