| Directory: | ./ |
|---|---|
| File: | src/steering-method/cross-state-optimization.cc |
| Date: | 2025-05-07 11:07:45 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 10 | 325 | 3.1% |
| Branches: | 20 | 680 | 2.9% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2017, Joseph Mirabel | ||
| 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr), | ||
| 3 | // Florent Lamiraux (florent.lamiraux@laas.fr) | ||
| 4 | // | ||
| 5 | |||
| 6 | // Redistribution and use in source and binary forms, with or without | ||
| 7 | // modification, are permitted provided that the following conditions are | ||
| 8 | // met: | ||
| 9 | // | ||
| 10 | // 1. Redistributions of source code must retain the above copyright | ||
| 11 | // notice, this list of conditions and the following disclaimer. | ||
| 12 | // | ||
| 13 | // 2. Redistributions in binary form must reproduce the above copyright | ||
| 14 | // notice, this list of conditions and the following disclaimer in the | ||
| 15 | // documentation and/or other materials provided with the distribution. | ||
| 16 | // | ||
| 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
| 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
| 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
| 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
| 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
| 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
| 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
| 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
| 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
| 28 | // DAMAGE. | ||
| 29 | |||
| 30 | #include <hpp/constraints/affine-function.hh> | ||
| 31 | #include <hpp/constraints/explicit.hh> | ||
| 32 | #include <hpp/constraints/locked-joint.hh> | ||
| 33 | #include <hpp/constraints/solver/by-substitution.hh> | ||
| 34 | #include <hpp/core/configuration-shooter.hh> | ||
| 35 | #include <hpp/core/path-vector.hh> | ||
| 36 | #include <hpp/manipulation/graph/edge.hh> | ||
| 37 | #include <hpp/manipulation/graph/state.hh> | ||
| 38 | #include <hpp/manipulation/steering-method/cross-state-optimization.hh> | ||
| 39 | #include <hpp/pinocchio/configuration.hh> | ||
| 40 | #include <hpp/pinocchio/joint-collection.hh> | ||
| 41 | #include <hpp/util/exception-factory.hh> | ||
| 42 | #include <map> | ||
| 43 | #include <pinocchio/multibody/model.hpp> | ||
| 44 | #include <queue> | ||
| 45 | #include <vector> | ||
| 46 | |||
| 47 | namespace hpp { | ||
| 48 | namespace manipulation { | ||
| 49 | namespace steeringMethod { | ||
| 50 | using Eigen::ColBlockIndices; | ||
| 51 | using Eigen::RowBlockIndices; | ||
| 52 | |||
| 53 | using graph::EdgePtr_t; | ||
| 54 | using graph::Edges_t; | ||
| 55 | using graph::LockedJoints_t; | ||
| 56 | using graph::Neighbors_t; | ||
| 57 | using graph::NumericalConstraints_t; | ||
| 58 | using graph::segments_t; | ||
| 59 | using graph::StatePtr_t; | ||
| 60 | using graph::States_t; | ||
| 61 | |||
| 62 | ✗ | CrossStateOptimizationPtr_t CrossStateOptimization::create( | |
| 63 | const ProblemConstPtr_t& problem) { | ||
| 64 | ✗ | CrossStateOptimizationPtr_t shPtr(new CrossStateOptimization(problem)); | |
| 65 | ✗ | shPtr->init(shPtr); | |
| 66 | ✗ | return shPtr; | |
| 67 | } | ||
| 68 | |||
| 69 | ✗ | CrossStateOptimizationPtr_t CrossStateOptimization::create( | |
| 70 | const core::ProblemConstPtr_t& problem) { | ||
| 71 | ✗ | assert(HPP_DYNAMIC_PTR_CAST(const Problem, problem)); | |
| 72 | ✗ | ProblemConstPtr_t p(HPP_STATIC_PTR_CAST(const Problem, problem)); | |
| 73 | ✗ | return create(p); | |
| 74 | } | ||
| 75 | |||
| 76 | ✗ | core::SteeringMethodPtr_t CrossStateOptimization::copy() const { | |
| 77 | ✗ | CrossStateOptimization* ptr = new CrossStateOptimization(*this); | |
| 78 | ✗ | CrossStateOptimizationPtr_t shPtr(ptr); | |
| 79 | ✗ | ptr->init(shPtr); | |
| 80 | ✗ | return shPtr; | |
| 81 | } | ||
| 82 | |||
| 83 | struct CrossStateOptimization::GraphSearchData { | ||
| 84 | StatePtr_t s1, s2; | ||
| 85 | |||
| 86 | // Datas for findNextTransitions | ||
| 87 | struct state_with_depth { | ||
| 88 | StatePtr_t s; | ||
| 89 | EdgePtr_t e; | ||
| 90 | std::size_t l; // depth to root | ||
| 91 | std::size_t i; // index in parent state_with_depths_t | ||
| 92 | ✗ | inline state_with_depth() : s(), e(), l(0), i(0) {} | |
| 93 | ✗ | inline state_with_depth(EdgePtr_t _e, std::size_t _l, std::size_t _i) | |
| 94 | ✗ | : s(_e->stateFrom()), e(_e), l(_l), i(_i) {} | |
| 95 | }; | ||
| 96 | typedef std::vector<state_with_depth> state_with_depths_t; | ||
| 97 | typedef std::map<StatePtr_t, state_with_depths_t> StateMap_t; | ||
| 98 | /// std::size_t is the index in state_with_depths_t at StateMap_t::iterator | ||
| 99 | struct state_with_depth_ptr_t { | ||
| 100 | StateMap_t::iterator state; | ||
| 101 | std::size_t parentIdx; | ||
| 102 | ✗ | state_with_depth_ptr_t(const StateMap_t::iterator& it, std::size_t idx) | |
| 103 | ✗ | : state(it), parentIdx(idx) {} | |
| 104 | }; | ||
| 105 | typedef std::queue<state_with_depth_ptr_t> Queue_t; | ||
| 106 | typedef std::set<EdgePtr_t> VisitedEdge_t; | ||
| 107 | std::size_t maxDepth; | ||
| 108 | StateMap_t parent1; // TODO, parent2; | ||
| 109 | Queue_t queue1; | ||
| 110 | VisitedEdge_t visitedEdge_; | ||
| 111 | |||
| 112 | ✗ | const state_with_depth& getParent(const state_with_depth_ptr_t& _p) const { | |
| 113 | ✗ | const state_with_depths_t& parents = _p.state->second; | |
| 114 | ✗ | return parents[_p.parentIdx]; | |
| 115 | } | ||
| 116 | |||
| 117 | ✗ | state_with_depth_ptr_t addInitState() { | |
| 118 | StateMap_t::iterator next = | ||
| 119 | ✗ | parent1.insert(StateMap_t::value_type(s1, state_with_depths_t(1))) | |
| 120 | ✗ | .first; | |
| 121 | ✗ | return state_with_depth_ptr_t(next, 0); | |
| 122 | } | ||
| 123 | |||
| 124 | ✗ | state_with_depth_ptr_t addParent(const state_with_depth_ptr_t& _p, | |
| 125 | const EdgePtr_t& transition) { | ||
| 126 | ✗ | const state_with_depths_t& parents = _p.state->second; | |
| 127 | ✗ | const state_with_depth& from = parents[_p.parentIdx]; | |
| 128 | |||
| 129 | // Insert state to if necessary | ||
| 130 | StateMap_t::iterator next = | ||
| 131 | parent1 | ||
| 132 | ✗ | .insert(StateMap_t::value_type(transition->stateTo(), | |
| 133 | ✗ | state_with_depths_t())) | |
| 134 | ✗ | .first; | |
| 135 | |||
| 136 | ✗ | next->second.push_back( | |
| 137 | ✗ | state_with_depth(transition, from.l + 1, _p.parentIdx)); | |
| 138 | |||
| 139 | ✗ | return state_with_depth_ptr_t(next, next->second.size() - 1); | |
| 140 | } | ||
| 141 | }; | ||
| 142 | |||
| 143 | ✗ | void CrossStateOptimization::gatherGraphConstraints() { | |
| 144 | typedef graph::Edge Edge; | ||
| 145 | typedef graph::EdgePtr_t EdgePtr_t; | ||
| 146 | typedef graph::GraphPtr_t GraphPtr_t; | ||
| 147 | typedef constraints::solver::BySubstitution Solver_t; | ||
| 148 | |||
| 149 | ✗ | GraphPtr_t cg(problem_->constraintGraph()); | |
| 150 | ✗ | const ConstraintsAndComplements_t& cac(cg->constraintsAndComplements()); | |
| 151 | ✗ | for (std::size_t i = 0; i < cg->nbComponents(); ++i) { | |
| 152 | ✗ | EdgePtr_t edge(HPP_DYNAMIC_PTR_CAST(Edge, cg->get(i).lock())); | |
| 153 | ✗ | if (edge) { | |
| 154 | const Solver_t& solver( | ||
| 155 | ✗ | edge->pathConstraint()->configProjector()->solver()); | |
| 156 | ✗ | const NumericalConstraints_t& constraints(solver.numericalConstraints()); | |
| 157 | ✗ | for (NumericalConstraints_t::const_iterator it(constraints.begin()); | |
| 158 | ✗ | it != constraints.end(); ++it) { | |
| 159 | ✗ | if ((*it)->parameterSize() > 0) { | |
| 160 | ✗ | const std::string& name((*it)->function().name()); | |
| 161 | ✗ | if (index_.find(name) == index_.end()) { | |
| 162 | // constraint is not in map, add it | ||
| 163 | ✗ | index_[name] = constraints_.size(); | |
| 164 | // Check whether constraint is equivalent to a previous one | ||
| 165 | ✗ | for (NumericalConstraints_t::const_iterator it1( | |
| 166 | ✗ | constraints_.begin()); | |
| 167 | ✗ | it1 != constraints_.end(); ++it1) { | |
| 168 | ✗ | for (ConstraintsAndComplements_t::const_iterator it2(cac.begin()); | |
| 169 | ✗ | it2 != cac.end(); ++it2) { | |
| 170 | ✗ | if (((**it1 == *(it2->complement)) && (**it == *(it2->both))) || | |
| 171 | ✗ | ((**it1 == *(it2->both)) && (**it == *(it2->complement)))) { | |
| 172 | ✗ | assert(sameRightHandSide_.count(*it1) == 0); | |
| 173 | ✗ | assert(sameRightHandSide_.count(*it) == 0); | |
| 174 | ✗ | sameRightHandSide_[*it1] = *it; | |
| 175 | ✗ | sameRightHandSide_[*it] = *it1; | |
| 176 | } | ||
| 177 | } | ||
| 178 | } | ||
| 179 | ✗ | constraints_.push_back(*it); | |
| 180 | hppDout(info, "Adding constraint \"" << name << "\""); | ||
| 181 | hppDout(info, "Edge \"" << edge->name() << "\""); | ||
| 182 | hppDout(info, "parameter size: " << (*it)->parameterSize()); | ||
| 183 | } | ||
| 184 | } | ||
| 185 | } | ||
| 186 | } | ||
| 187 | } | ||
| 188 | } | ||
| 189 | |||
| 190 | ✗ | bool CrossStateOptimization::findTransitions(GraphSearchData& d) const { | |
| 191 | ✗ | while (!d.queue1.empty()) { | |
| 192 | ✗ | GraphSearchData::state_with_depth_ptr_t _state = d.queue1.front(); | |
| 193 | |||
| 194 | ✗ | const GraphSearchData::state_with_depth& parent = d.getParent(_state); | |
| 195 | ✗ | if (parent.l >= d.maxDepth) return true; | |
| 196 | ✗ | d.queue1.pop(); | |
| 197 | |||
| 198 | ✗ | bool done = false; | |
| 199 | |||
| 200 | ✗ | const Neighbors_t& neighbors = _state.state->first->neighbors(); | |
| 201 | ✗ | for (Neighbors_t::const_iterator _n = neighbors.begin(); | |
| 202 | ✗ | _n != neighbors.end(); ++_n) { | |
| 203 | ✗ | EdgePtr_t transition = _n->second; | |
| 204 | |||
| 205 | // Avoid identical consecutive transition | ||
| 206 | ✗ | if (transition == parent.e) continue; | |
| 207 | |||
| 208 | // If transition has already been visited, continue | ||
| 209 | // if (d.visitedEdge_.count (transition) == 1) continue; | ||
| 210 | |||
| 211 | // TODO | ||
| 212 | // If (transition->to() == d.s2) check if this list is feasible. | ||
| 213 | // - If a constraint with non-constant right hand side is present | ||
| 214 | // in all transitions, then the rhs from d.q1 and d.q2 should be | ||
| 215 | // equal | ||
| 216 | |||
| 217 | // Insert parent | ||
| 218 | ✗ | d.queue1.push(d.addParent(_state, transition)); | |
| 219 | |||
| 220 | ✗ | done = done || (transition->stateTo() == d.s2); | |
| 221 | } | ||
| 222 | ✗ | if (done) break; | |
| 223 | } | ||
| 224 | ✗ | return false; | |
| 225 | } | ||
| 226 | |||
| 227 | ✗ | Edges_t CrossStateOptimization::getTransitionList(GraphSearchData& d, | |
| 228 | const std::size_t& i) const { | ||
| 229 | ✗ | assert(d.parent1.find(d.s2) != d.parent1.end()); | |
| 230 | ✗ | const GraphSearchData::state_with_depths_t& roots = d.parent1[d.s2]; | |
| 231 | ✗ | Edges_t transitions; | |
| 232 | ✗ | if (i >= roots.size()) return transitions; | |
| 233 | |||
| 234 | ✗ | const GraphSearchData::state_with_depth* current = &roots[i]; | |
| 235 | ✗ | transitions.reserve(current->l); | |
| 236 | ✗ | graph::WaypointEdgePtr_t we; | |
| 237 | ✗ | while (current->e) { | |
| 238 | ✗ | assert(current->l > 0); | |
| 239 | ✗ | we = HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, current->e); | |
| 240 | ✗ | if (we) { | |
| 241 | ✗ | for (int i = (int)we->nbWaypoints(); i >= 0; --i) | |
| 242 | ✗ | transitions.push_back(we->waypoint(i)); | |
| 243 | } else { | ||
| 244 | ✗ | transitions.push_back(current->e); | |
| 245 | } | ||
| 246 | ✗ | current = &d.parent1[current->s][current->i]; | |
| 247 | } | ||
| 248 | ✗ | std::reverse(transitions.begin(), transitions.end()); | |
| 249 | ✗ | return transitions; | |
| 250 | } | ||
| 251 | |||
| 252 | namespace internal { | ||
| 253 | ✗ | bool saturate(const core::DevicePtr_t& robot, vectorIn_t q, vectorOut_t qSat, | |
| 254 | pinocchio::ArrayXb& saturatedDof) { | ||
| 255 | ✗ | qSat = q; | |
| 256 | ✗ | return hpp::pinocchio::saturate(robot, qSat, saturatedDof); | |
| 257 | } | ||
| 258 | } // namespace internal | ||
| 259 | |||
| 260 | struct CrossStateOptimization::OptimizationData { | ||
| 261 | typedef constraints::solver::HierarchicalIterative::Saturation_t Saturation_t; | ||
| 262 | enum RightHandSideStatus_t { | ||
| 263 | // Constraint is not in solver for this waypoint | ||
| 264 | ABSENT, | ||
| 265 | // right hand side of constraint for this waypoint is equal to | ||
| 266 | // right hand side for previous waypoint | ||
| 267 | EQUAL_TO_PREVIOUS, | ||
| 268 | // right hand side of constraint for this waypoint is equal to | ||
| 269 | // right hand side for initial configuration | ||
| 270 | EQUAL_TO_INIT, | ||
| 271 | // right hand side of constraint for this waypoint is equal to | ||
| 272 | // right hand side for goal configuration | ||
| 273 | EQUAL_TO_GOAL | ||
| 274 | }; // enum RightHandSideStatus_t | ||
| 275 | const std::size_t N, nq, nv; | ||
| 276 | std::vector<Solver_t> solvers; | ||
| 277 | // Waypoints lying in each intermediate state | ||
| 278 | matrix_t waypoint; | ||
| 279 | // Initial guess of each solver stored as matrix columns | ||
| 280 | matrix_t qInit; | ||
| 281 | Configuration_t q1, q2; | ||
| 282 | core::DevicePtr_t robot; | ||
| 283 | // Matrix specifying for each constraint and each waypoint how | ||
| 284 | // the right hand side is initialized in the solver. | ||
| 285 | Eigen::Matrix<LiegroupElement, Eigen::Dynamic, Eigen::Dynamic> M_rhs; | ||
| 286 | Eigen::Matrix<RightHandSideStatus_t, Eigen::Dynamic, Eigen::Dynamic> M_status; | ||
| 287 | // Number of trials to generate each waypoint configuration | ||
| 288 | ✗ | OptimizationData(const core::ProblemConstPtr_t _problem, | |
| 289 | const Configuration_t& _q1, const Configuration_t& _q2, | ||
| 290 | const Edges_t& transitions) | ||
| 291 | ✗ | : N(transitions.size() - 1), | |
| 292 | ✗ | nq(_problem->robot()->configSize()), | |
| 293 | ✗ | nv(_problem->robot()->numberDof()), | |
| 294 | ✗ | solvers(N, _problem->robot()->configSpace()), | |
| 295 | ✗ | waypoint(nq, N), | |
| 296 | ✗ | qInit(nq, N), | |
| 297 | ✗ | q1(_q1), | |
| 298 | ✗ | q2(_q2), | |
| 299 | ✗ | robot(_problem->robot()), | |
| 300 | ✗ | M_rhs(), | |
| 301 | ✗ | M_status() { | |
| 302 | ✗ | for (auto solver : solvers) { | |
| 303 | // Set maximal number of iterations for each solver | ||
| 304 | ✗ | solver.maxIterations( | |
| 305 | ✗ | _problem->getParameter("CrossStateOptimization/maxIteration") | |
| 306 | .intValue()); | ||
| 307 | // Set error threshold for each solver | ||
| 308 | ✗ | solver.errorThreshold( | |
| 309 | ✗ | _problem->getParameter("CrossStateOptimization/errorThreshold") | |
| 310 | ✗ | .floatValue()); | |
| 311 | } | ||
| 312 | ✗ | assert(transitions.size() > 0); | |
| 313 | } | ||
| 314 | }; | ||
| 315 | |||
| 316 | ✗ | bool CrossStateOptimization::checkConstantRightHandSide(OptimizationData& d, | |
| 317 | size_type index) const { | ||
| 318 | ✗ | const ImplicitPtr_t c(constraints_[index]); | |
| 319 | ✗ | LiegroupElement rhsInit(c->function().outputSpace()); | |
| 320 | ✗ | c->rightHandSideFromConfig(d.q1, rhsInit); | |
| 321 | ✗ | LiegroupElement rhsGoal(c->function().outputSpace()); | |
| 322 | ✗ | c->rightHandSideFromConfig(d.q2, rhsGoal); | |
| 323 | // Check that right hand sides are close to each other | ||
| 324 | ✗ | value_type eps(problem_->constraintGraph()->errorThreshold()); | |
| 325 | ✗ | value_type eps2(eps * eps); | |
| 326 | ✗ | if ((rhsGoal - rhsInit).squaredNorm() > eps2) { | |
| 327 | ✗ | return false; | |
| 328 | } | ||
| 329 | // Matrix of solver right hand sides | ||
| 330 | ✗ | for (size_type j = 0; j < d.M_rhs.cols(); ++j) { | |
| 331 | ✗ | d.M_rhs(index, j) = rhsInit; | |
| 332 | } | ||
| 333 | ✗ | return true; | |
| 334 | } | ||
| 335 | |||
| 336 | ✗ | void displayRhsMatrix( | |
| 337 | const Eigen::Matrix<vector_t, Eigen::Dynamic, Eigen::Dynamic>& m, | ||
| 338 | const NumericalConstraints_t& constraints) { | ||
| 339 | ✗ | std::ostringstream oss; | |
| 340 | ✗ | oss.precision(5); | |
| 341 | ✗ | oss << "\\documentclass[12pt,landscape]{article}" << std::endl; | |
| 342 | ✗ | oss << "\\usepackage[a3paper]{geometry}" << std::endl; | |
| 343 | ✗ | oss << "\\begin {document}" << std::endl; | |
| 344 | ✗ | oss << "\\begin {tabular}{"; | |
| 345 | ✗ | for (size_type j = 0; j < m.cols() + 1; ++j) oss << "c"; | |
| 346 | ✗ | oss << "}" << std::endl; | |
| 347 | ✗ | for (size_type i = 0; i < m.rows(); ++i) { | |
| 348 | ✗ | oss << constraints[i]->function().name() << " & "; | |
| 349 | ✗ | for (size_type j = 0; j < m.cols(); ++j) { | |
| 350 | ✗ | oss << "$\\left(\\begin{array}{c}" << std::endl; | |
| 351 | ✗ | for (size_type k = 0; k < m(i, j).size(); ++k) { | |
| 352 | ✗ | oss << m(i, j)[k] << "\\\\" << std::endl; | |
| 353 | } | ||
| 354 | ✗ | oss << "\\end{array}\\right)$" << std::endl; | |
| 355 | ✗ | if (j < m.cols() - 1) { | |
| 356 | ✗ | oss << " & " << std::endl; | |
| 357 | } | ||
| 358 | } | ||
| 359 | ✗ | oss << "\\\\" << std::endl; | |
| 360 | } | ||
| 361 | ✗ | oss << "\\end{tabular}" << std::endl; | |
| 362 | ✗ | oss << "\\end {document}" << std::endl; | |
| 363 | hppDout(info, oss.str()); | ||
| 364 | } | ||
| 365 | |||
| 366 | ✗ | void displayStatusMatrix( | |
| 367 | const Eigen::Matrix< | ||
| 368 | CrossStateOptimization::OptimizationData::RightHandSideStatus_t, | ||
| 369 | Eigen::Dynamic, Eigen::Dynamic>& m, | ||
| 370 | const NumericalConstraints_t& constraints, | ||
| 371 | const graph::Edges_t& transitions) { | ||
| 372 | ✗ | std::ostringstream oss; | |
| 373 | ✗ | oss.precision(5); | |
| 374 | ✗ | oss << "\\documentclass[12pt,landscape]{article}" << std::endl; | |
| 375 | ✗ | oss << "\\usepackage[a3paper]{geometry}" << std::endl; | |
| 376 | ✗ | oss << "\\begin {document}" << std::endl; | |
| 377 | ✗ | oss << "\\paragraph{Edges}" << std::endl; | |
| 378 | ✗ | oss << "\\begin{enumerate}" << std::endl; | |
| 379 | ✗ | for (auto edge : transitions) { | |
| 380 | ✗ | oss << "\\item " << edge->name() << std::endl; | |
| 381 | } | ||
| 382 | ✗ | oss << "\\end{enumerate}" << std::endl; | |
| 383 | ✗ | oss << "\\begin {tabular}{"; | |
| 384 | ✗ | for (size_type j = 0; j < m.cols() + 1; ++j) oss << "c"; | |
| 385 | ✗ | oss << "}" << std::endl; | |
| 386 | ✗ | for (size_type i = 0; i < m.rows(); ++i) { | |
| 387 | ✗ | oss << constraints[i]->function().name() << " & "; | |
| 388 | ✗ | for (size_type j = 0; j < m.cols(); ++j) { | |
| 389 | ✗ | oss << m(i, j); | |
| 390 | ✗ | if (j < m.cols() - 1) { | |
| 391 | ✗ | oss << " & " << std::endl; | |
| 392 | } | ||
| 393 | } | ||
| 394 | ✗ | oss << "\\\\" << std::endl; | |
| 395 | } | ||
| 396 | ✗ | oss << "\\end{tabular}" << std::endl; | |
| 397 | ✗ | oss << "\\end {document}" << std::endl; | |
| 398 | hppDout(info, oss.str()); | ||
| 399 | } | ||
| 400 | |||
| 401 | ✗ | bool CrossStateOptimization::contains(const Solver_t& solver, | |
| 402 | const ImplicitPtr_t& c) const { | ||
| 403 | ✗ | if (solver.contains(c)) return true; | |
| 404 | std::map<ImplicitPtr_t, ImplicitPtr_t>::const_iterator it( | ||
| 405 | ✗ | sameRightHandSide_.find(c)); | |
| 406 | ✗ | if ((it != sameRightHandSide_.end() && solver.contains(it->second))) { | |
| 407 | ✗ | return true; | |
| 408 | } | ||
| 409 | ✗ | return false; | |
| 410 | } | ||
| 411 | |||
| 412 | ✗ | bool CrossStateOptimization::buildOptimizationProblem( | |
| 413 | OptimizationData& d, const graph::Edges_t& transitions) const { | ||
| 414 | ✗ | if (d.N == 0) return true; | |
| 415 | ✗ | d.M_status.resize(constraints_.size(), d.N); | |
| 416 | ✗ | d.M_status.fill(OptimizationData::ABSENT); | |
| 417 | ✗ | d.M_rhs.resize(constraints_.size(), d.N); | |
| 418 | ✗ | d.M_rhs.fill(LiegroupElement()); | |
| 419 | ✗ | size_type index = 0; | |
| 420 | // Loop over constraints | ||
| 421 | ✗ | for (NumericalConstraints_t::const_iterator it(constraints_.begin()); | |
| 422 | ✗ | it != constraints_.end(); ++it) { | |
| 423 | ✗ | const ImplicitPtr_t& c(*it); | |
| 424 | // Loop forward over waypoints to determine right hand sides equal | ||
| 425 | // to initial configuration | ||
| 426 | ✗ | for (std::size_t j = 0; j < d.N; ++j) { | |
| 427 | // Get transition solver | ||
| 428 | const Solver_t& trSolver( | ||
| 429 | ✗ | transitions[j]->pathConstraint()->configProjector()->solver()); | |
| 430 | ✗ | if (contains(trSolver, c)) { | |
| 431 | ✗ | if ((j == 0) || | |
| 432 | ✗ | d.M_status(index, j - 1) == OptimizationData::EQUAL_TO_INIT) { | |
| 433 | ✗ | d.M_status(index, j) = OptimizationData::EQUAL_TO_INIT; | |
| 434 | } else { | ||
| 435 | ✗ | d.M_status(index, j) = OptimizationData::EQUAL_TO_PREVIOUS; | |
| 436 | } | ||
| 437 | } | ||
| 438 | } | ||
| 439 | // Loop backward over waypoints to determine right hand sides equal | ||
| 440 | // to final configuration | ||
| 441 | ✗ | for (size_type j = d.N - 1; j > 0; --j) { | |
| 442 | // Get transition solver | ||
| 443 | ✗ | const Solver_t& trSolver(transitions[(std::size_t)j + 1] | |
| 444 | ✗ | ->pathConstraint() | |
| 445 | ✗ | ->configProjector() | |
| 446 | ✗ | ->solver()); | |
| 447 | ✗ | if (contains(trSolver, c)) { | |
| 448 | ✗ | if ((j == (size_type)d.N - 1) || | |
| 449 | ✗ | d.M_status(index, j + 1) == OptimizationData::EQUAL_TO_GOAL) { | |
| 450 | // If constraint right hand side is already equal to | ||
| 451 | // initial config, check that right hand side is equal | ||
| 452 | // for init and goal configs. | ||
| 453 | ✗ | if (d.M_status(index, j) == OptimizationData::EQUAL_TO_INIT) { | |
| 454 | ✗ | if (checkConstantRightHandSide(d, index)) { | |
| 455 | // stop for this constraint | ||
| 456 | ✗ | break; | |
| 457 | } else { | ||
| 458 | // Right hand side of constraint should be equal along the | ||
| 459 | // whole path but is different at init and at goal configs. | ||
| 460 | ✗ | return false; | |
| 461 | } | ||
| 462 | } else { | ||
| 463 | ✗ | d.M_status(index, j) = OptimizationData::EQUAL_TO_GOAL; | |
| 464 | } | ||
| 465 | } | ||
| 466 | } | ||
| 467 | } | ||
| 468 | ✗ | ++index; | |
| 469 | } // for (NumericalConstraints_t::const_iterator it | ||
| 470 | ✗ | displayStatusMatrix(d.M_status, constraints_, transitions); | |
| 471 | ✗ | graph::GraphPtr_t cg(problem_->constraintGraph()); | |
| 472 | // Fill solvers with target constraints of transition | ||
| 473 | ✗ | for (std::size_t j = 0; j < d.N; ++j) { | |
| 474 | ✗ | d.solvers[(std::size_t)j] = transitions[(std::size_t)j] | |
| 475 | ✗ | ->targetConstraint() | |
| 476 | ✗ | ->configProjector() | |
| 477 | ✗ | ->solver(); | |
| 478 | } | ||
| 479 | // Initial guess | ||
| 480 | ✗ | std::vector<size_type> ks; | |
| 481 | ✗ | size_type K = 0; | |
| 482 | ✗ | ks.resize(d.N); | |
| 483 | ✗ | for (std::size_t i = 0; i < d.N + 1; ++i) { | |
| 484 | ✗ | if (!transitions[i]->isShort()) ++K; | |
| 485 | ✗ | if (i < d.N) ks[i] = K; | |
| 486 | } | ||
| 487 | ✗ | if (K == 0) { | |
| 488 | ✗ | ++K; | |
| 489 | ✗ | for (std::size_t i = d.N / 2; i < d.N; ++i) ks[i] = 1; | |
| 490 | } | ||
| 491 | ✗ | for (std::size_t i = 0; i < d.N; ++i) { | |
| 492 | ✗ | value_type u = value_type(ks[i]) / value_type(K); | |
| 493 | ✗ | pinocchio::interpolate(d.robot, d.q1, d.q2, u, d.qInit.col(i)); | |
| 494 | hppDout(info, "qInit = " << pinocchio::displayConfig(d.qInit.col(i))); | ||
| 495 | } | ||
| 496 | |||
| 497 | ✗ | return true; | |
| 498 | } | ||
| 499 | |||
| 500 | ✗ | bool CrossStateOptimization::solveOptimizationProblem( | |
| 501 | OptimizationData& d) const { | ||
| 502 | // Iterate on waypoint solvers, for each of them | ||
| 503 | // 1. initialize right hand side, | ||
| 504 | // 2. solve. | ||
| 505 | ✗ | for (std::size_t j = 0; j < d.solvers.size(); ++j) { | |
| 506 | ✗ | Solver_t& solver(d.solvers[j]); | |
| 507 | ✗ | for (std::size_t i = 0; i < constraints_.size(); ++i) { | |
| 508 | ✗ | const ImplicitPtr_t& c(constraints_[i]); | |
| 509 | ✗ | switch (d.M_status((size_type)i, (size_type)j)) { | |
| 510 | ✗ | case OptimizationData::EQUAL_TO_PREVIOUS: | |
| 511 | ✗ | assert(j != 0); | |
| 512 | ✗ | solver.rightHandSideFromConfig(c, d.waypoint.col(j - 1)); | |
| 513 | ✗ | break; | |
| 514 | ✗ | case OptimizationData::EQUAL_TO_INIT: | |
| 515 | ✗ | solver.rightHandSideFromConfig(c, d.q1); | |
| 516 | ✗ | break; | |
| 517 | ✗ | case OptimizationData::EQUAL_TO_GOAL: | |
| 518 | ✗ | solver.rightHandSideFromConfig(c, d.q2); | |
| 519 | ✗ | break; | |
| 520 | ✗ | case OptimizationData::ABSENT: | |
| 521 | default:; | ||
| 522 | } | ||
| 523 | } | ||
| 524 | ✗ | if (j == 0) | |
| 525 | ✗ | d.waypoint.col(j) = d.qInit.col(j); | |
| 526 | else | ||
| 527 | ✗ | d.waypoint.col(j) = d.waypoint.col(j - 1); | |
| 528 | ✗ | Solver_t::Status status = solver.solve( | |
| 529 | ✗ | d.waypoint.col(j), constraints::solver::lineSearch::Backtracking()); | |
| 530 | ✗ | size_type nbTry = 0; | |
| 531 | size_type nRandomConfigs( | ||
| 532 | ✗ | problem() | |
| 533 | ✗ | ->getParameter("CrossStateOptimization/nRandomConfigs") | |
| 534 | ✗ | .intValue()); | |
| 535 | |||
| 536 | ✗ | while (status != Solver_t::SUCCESS && nbTry < nRandomConfigs) { | |
| 537 | ✗ | d.waypoint.col(j) = problem()->configurationShooter()->shoot(); | |
| 538 | ✗ | status = solver.solve(d.waypoint.col(j), | |
| 539 | ✗ | constraints::solver::lineSearch::Backtracking()); | |
| 540 | ✗ | ++nbTry; | |
| 541 | } | ||
| 542 | ✗ | switch (status) { | |
| 543 | ✗ | case Solver_t::ERROR_INCREASED: | |
| 544 | hppDout(info, "error increased."); | ||
| 545 | ✗ | return false; | |
| 546 | ✗ | case Solver_t::MAX_ITERATION_REACHED: | |
| 547 | hppDout(info, "max iteration reached."); | ||
| 548 | ✗ | return false; | |
| 549 | ✗ | case Solver_t::INFEASIBLE: | |
| 550 | hppDout(info, "infeasible."); | ||
| 551 | ✗ | return false; | |
| 552 | ✗ | case Solver_t::SUCCESS: | |
| 553 | hppDout(info, "success."); | ||
| 554 | } | ||
| 555 | } | ||
| 556 | ✗ | return true; | |
| 557 | } | ||
| 558 | |||
| 559 | ✗ | core::PathVectorPtr_t CrossStateOptimization::buildPath( | |
| 560 | OptimizationData& d, const Edges_t& transitions) const { | ||
| 561 | using core::PathVector; | ||
| 562 | using core::PathVectorPtr_t; | ||
| 563 | |||
| 564 | ✗ | const core::DevicePtr_t& robot = problem()->robot(); | |
| 565 | PathVectorPtr_t pv = | ||
| 566 | ✗ | PathVector::create(robot->configSize(), robot->numberDof()); | |
| 567 | ✗ | core::PathPtr_t path; | |
| 568 | |||
| 569 | ✗ | std::size_t i = 0; | |
| 570 | ✗ | for (Edges_t::const_iterator _t = transitions.begin(); | |
| 571 | ✗ | _t != transitions.end(); ++_t) { | |
| 572 | ✗ | const EdgePtr_t& t = *_t; | |
| 573 | ✗ | bool first = (i == 0); | |
| 574 | ✗ | bool last = (i == d.N); | |
| 575 | |||
| 576 | bool status; | ||
| 577 | ✗ | if (first && last) | |
| 578 | ✗ | status = t->build(path, d.q1, d.q2); | |
| 579 | ✗ | else if (first) | |
| 580 | ✗ | status = t->build(path, d.q1, d.waypoint.col(i)); | |
| 581 | ✗ | else if (last) | |
| 582 | ✗ | status = t->build(path, d.waypoint.col(i - 1), d.q2); | |
| 583 | else { | ||
| 584 | ✗ | status = t->build(path, d.waypoint.col(i - 1), d.waypoint.col(i)); | |
| 585 | } | ||
| 586 | |||
| 587 | ✗ | if (!status || !path) { | |
| 588 | hppDout(warning, "Could not build path from solution "); | ||
| 589 | ✗ | return PathVectorPtr_t(); | |
| 590 | } | ||
| 591 | ✗ | pv->appendPath(path); | |
| 592 | |||
| 593 | ✗ | ++i; | |
| 594 | } | ||
| 595 | ✗ | return pv; | |
| 596 | } | ||
| 597 | |||
| 598 | ✗ | core::PathPtr_t CrossStateOptimization::impl_compute( | |
| 599 | ConfigurationIn_t q1, ConfigurationIn_t q2) const { | ||
| 600 | ✗ | const graph::GraphPtr_t& graph(problem_->constraintGraph()); | |
| 601 | ✗ | GraphSearchData d; | |
| 602 | ✗ | d.s1 = graph->getState(q1); | |
| 603 | ✗ | d.s2 = graph->getState(q2); | |
| 604 | // d.maxDepth = 2; | ||
| 605 | ✗ | d.maxDepth = | |
| 606 | ✗ | problem_->getParameter("CrossStateOptimization/maxDepth").intValue(); | |
| 607 | |||
| 608 | // Find | ||
| 609 | ✗ | d.queue1.push(d.addInitState()); | |
| 610 | ✗ | std::size_t idxSol = (d.s1 == d.s2 ? 1 : 0); | |
| 611 | ✗ | bool maxDepthReached = findTransitions(d); | |
| 612 | |||
| 613 | ✗ | while (!maxDepthReached) { | |
| 614 | ✗ | Edges_t transitions = getTransitionList(d, idxSol); | |
| 615 | ✗ | while (!transitions.empty()) { | |
| 616 | #ifdef HPP_DEBUG | ||
| 617 | std::ostringstream ss; | ||
| 618 | ss << "Trying solution " << idxSol << ": "; | ||
| 619 | for (std::size_t j = 0; j < transitions.size(); ++j) | ||
| 620 | ss << transitions[j]->name() << ", "; | ||
| 621 | hppDout(info, ss.str()); | ||
| 622 | #endif // HPP_DEBUG | ||
| 623 | |||
| 624 | ✗ | OptimizationData optData(problem(), q1, q2, transitions); | |
| 625 | ✗ | if (buildOptimizationProblem(optData, transitions)) { | |
| 626 | ✗ | if (solveOptimizationProblem(optData)) { | |
| 627 | ✗ | core::PathPtr_t path = buildPath(optData, transitions); | |
| 628 | ✗ | if (path) return path; | |
| 629 | hppDout(info, "Failed to build path from solution: "); | ||
| 630 | ✗ | } else { | |
| 631 | hppDout(info, "Failed to solve"); | ||
| 632 | } | ||
| 633 | } | ||
| 634 | ✗ | ++idxSol; | |
| 635 | ✗ | transitions = getTransitionList(d, idxSol); | |
| 636 | } | ||
| 637 | ✗ | maxDepthReached = findTransitions(d); | |
| 638 | } | ||
| 639 | |||
| 640 | ✗ | return core::PathPtr_t(); | |
| 641 | } | ||
| 642 | |||
| 643 | using core::Parameter; | ||
| 644 | using core::ParameterDescription; | ||
| 645 | |||
| 646 | 1 | HPP_START_PARAMETER_DECLARATION(CrossStateOptimization) | |
| 647 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
1 | core::Problem::declareParameter(ParameterDescription( |
| 648 | Parameter::INT, "CrossStateOptimization/maxDepth", | ||
| 649 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "Maximum number of transitions to look for.", Parameter((size_type)2))); |
| 650 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
1 | core::Problem::declareParameter(ParameterDescription( |
| 651 | Parameter::INT, "CrossStateOptimization/maxIteration", | ||
| 652 | "Maximum number of iterations of the Newton Raphson algorithm.", | ||
| 653 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Parameter((size_type)60))); |
| 654 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
1 | core::Problem::declareParameter(ParameterDescription( |
| 655 | Parameter::FLOAT, "CrossStateOptimization/errorThreshold", | ||
| 656 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "Error threshold of the Newton Raphson algorithm.", Parameter(1e-4))); |
| 657 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
1 | core::Problem::declareParameter(ParameterDescription( |
| 658 | Parameter::INT, "CrossStateOptimization/nRandomConfigs", | ||
| 659 | "Number of random configurations to sample to initialize each " | ||
| 660 | "solver.", | ||
| 661 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Parameter((size_type)0))); |
| 662 | 1 | HPP_END_PARAMETER_DECLARATION(CrossStateOptimization) | |
| 663 | } // namespace steeringMethod | ||
| 664 | } // namespace manipulation | ||
| 665 | } // namespace hpp | ||
| 666 |