| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// Copyright (c) 2014, LAAS-CNRS |
| 2 |
|
|
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) |
| 3 |
|
|
// |
| 4 |
|
|
|
| 5 |
|
|
// Redistribution and use in source and binary forms, with or without |
| 6 |
|
|
// modification, are permitted provided that the following conditions are |
| 7 |
|
|
// met: |
| 8 |
|
|
// |
| 9 |
|
|
// 1. Redistributions of source code must retain the above copyright |
| 10 |
|
|
// notice, this list of conditions and the following disclaimer. |
| 11 |
|
|
// |
| 12 |
|
|
// 2. Redistributions in binary form must reproduce the above copyright |
| 13 |
|
|
// notice, this list of conditions and the following disclaimer in the |
| 14 |
|
|
// documentation and/or other materials provided with the distribution. |
| 15 |
|
|
// |
| 16 |
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 17 |
|
|
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 18 |
|
|
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
| 19 |
|
|
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT |
| 20 |
|
|
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
| 21 |
|
|
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
| 22 |
|
|
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, |
| 23 |
|
|
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY |
| 24 |
|
|
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
| 25 |
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
| 26 |
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH |
| 27 |
|
|
// DAMAGE. |
| 28 |
|
|
|
| 29 |
|
|
#include "graph.impl.hh" |
| 30 |
|
|
|
| 31 |
|
|
#include <fstream> |
| 32 |
|
|
#include <hpp/constraints/differentiable-function.hh> |
| 33 |
|
|
#include <hpp/corbaserver/conversions.hh> |
| 34 |
|
|
#include <hpp/corbaserver/manipulation/server.hh> |
| 35 |
|
|
#include <hpp/manipulation/connected-component.hh> |
| 36 |
|
|
#include <hpp/manipulation/constraint-set.hh> |
| 37 |
|
|
#include <hpp/manipulation/graph/edge.hh> |
| 38 |
|
|
#include <hpp/manipulation/graph/graph.hh> |
| 39 |
|
|
#include <hpp/manipulation/graph/guided-state-selector.hh> |
| 40 |
|
|
#include <hpp/manipulation/graph/helper.hh> |
| 41 |
|
|
#include <hpp/manipulation/graph/state-selector.hh> |
| 42 |
|
|
#include <hpp/manipulation/graph/state.hh> |
| 43 |
|
|
#include <hpp/manipulation/manipulation-planner.hh> |
| 44 |
|
|
#include <hpp/manipulation/problem.hh> |
| 45 |
|
|
#include <hpp/manipulation/roadmap.hh> |
| 46 |
|
|
#include <hpp/manipulation/steering-method/graph.hh> |
| 47 |
|
|
#include <hpp/util/debug.hh> |
| 48 |
|
|
#include <hpp/util/exception-factory.hh> |
| 49 |
|
|
#include <hpp/util/pointer.hh> |
| 50 |
|
|
#include <pinocchio/multibody/model.hpp> |
| 51 |
|
|
#include <sstream> |
| 52 |
|
|
|
| 53 |
|
|
#include "tools.hh" |
| 54 |
|
|
|
| 55 |
|
|
namespace hpp { |
| 56 |
|
|
namespace manipulation { |
| 57 |
|
|
using constraints::Implicit; |
| 58 |
|
|
namespace impl { |
| 59 |
|
|
using CORBA::ULong; |
| 60 |
|
|
using corbaServer::floatSeqToConfig; |
| 61 |
|
|
using graph::Edge; |
| 62 |
|
|
using graph::EdgePtr_t; |
| 63 |
|
|
using graph::LevelSetEdge; |
| 64 |
|
|
using graph::LevelSetEdgePtr_t; |
| 65 |
|
|
using graph::WaypointEdge; |
| 66 |
|
|
using graph::WaypointEdgePtr_t; |
| 67 |
|
|
|
| 68 |
|
|
namespace { |
| 69 |
|
|
typedef core::ProblemSolver CPs_t; |
| 70 |
|
|
|
| 71 |
|
|
template <typename T> |
| 72 |
|
✗ |
std::string toStr() { |
| 73 |
|
✗ |
return typeid(T).name(); |
| 74 |
|
|
} |
| 75 |
|
|
template <> |
| 76 |
|
✗ |
std::string toStr<graph::State>() { |
| 77 |
|
✗ |
return "Node"; |
| 78 |
|
|
} |
| 79 |
|
|
template <> |
| 80 |
|
✗ |
std::string toStr<graph::Edge>() { |
| 81 |
|
✗ |
return "Edge"; |
| 82 |
|
|
} |
| 83 |
|
|
template <> |
| 84 |
|
✗ |
std::string toStr<graph::Graph>() { |
| 85 |
|
✗ |
return "Graph"; |
| 86 |
|
|
} |
| 87 |
|
|
template <> |
| 88 |
|
✗ |
std::string toStr<graph::LevelSetEdge>() { |
| 89 |
|
✗ |
return "LevelSetEdge"; |
| 90 |
|
|
} |
| 91 |
|
|
template <> |
| 92 |
|
✗ |
std::string toStr<graph::WaypointEdge>() { |
| 93 |
|
✗ |
return "WaypointEdge"; |
| 94 |
|
|
} |
| 95 |
|
|
|
| 96 |
|
✗ |
std::vector<graph::helper::ObjectDef_t> toObjectVector(const Names_t& names, |
| 97 |
|
|
const Namess_t& hsPO, |
| 98 |
|
|
const Namess_t& shPO) { |
| 99 |
|
|
using graph::helper::ObjectDef_t; |
| 100 |
|
✗ |
std::vector<graph::helper::ObjectDef_t> ret; |
| 101 |
|
|
// Check size of lists |
| 102 |
|
✗ |
if (hsPO.length() != names.length()) { |
| 103 |
|
✗ |
HPP_THROW(Error, "Number of handle lists (" |
| 104 |
|
|
<< hsPO.length() |
| 105 |
|
|
<< ") does not match number of objects (" |
| 106 |
|
|
<< names.length() << ")."); |
| 107 |
|
|
} |
| 108 |
|
✗ |
if (shPO.length() != names.length()) { |
| 109 |
|
✗ |
HPP_THROW(Error, "Number of contact lists (" |
| 110 |
|
|
<< shPO.length() |
| 111 |
|
|
<< ") does not match number of objects (" |
| 112 |
|
|
<< names.length() << ")."); |
| 113 |
|
|
} |
| 114 |
|
✗ |
for (ULong i = 0; i < names.length(); ++i) { |
| 115 |
|
✗ |
ret.push_back(ObjectDef_t()); |
| 116 |
|
✗ |
ObjectDef_t& od = ret.back(); |
| 117 |
|
✗ |
od.name = names[i]; |
| 118 |
|
✗ |
od.handles = corbaServer::toStrings<std::vector<std::string> >(hsPO[i]); |
| 119 |
|
✗ |
od.shapes = corbaServer::toStrings<std::vector<std::string> >(shPO[i]); |
| 120 |
|
|
} |
| 121 |
|
✗ |
return ret; |
| 122 |
|
|
} |
| 123 |
|
|
|
| 124 |
|
✗ |
void setRule(const hpp::corbaserver::manipulation::Rule& in, |
| 125 |
|
|
graph::helper::Rule& out) { |
| 126 |
|
✗ |
out.grippers_ = toStringVector(in.grippers); |
| 127 |
|
✗ |
out.handles_ = toStringVector(in.handles); |
| 128 |
|
✗ |
out.link_ = in.link; |
| 129 |
|
|
} |
| 130 |
|
|
} // namespace |
| 131 |
|
|
|
| 132 |
|
✗ |
Graph::Graph() : server_(0x0) {} |
| 133 |
|
|
|
| 134 |
|
✗ |
ProblemSolverPtr_t Graph::problemSolver() { return server_->problemSolver(); } |
| 135 |
|
|
|
| 136 |
|
✗ |
graph::GraphPtr_t Graph::graph(bool throwIfNull) { |
| 137 |
|
✗ |
graph::GraphPtr_t g = problemSolver()->constraintGraph(); |
| 138 |
|
✗ |
if (throwIfNull && !g) throw Error("You should create the graph"); |
| 139 |
|
✗ |
return g; |
| 140 |
|
|
} |
| 141 |
|
|
|
| 142 |
|
|
template <typename T> |
| 143 |
|
✗ |
shared_ptr<T> Graph::getComp(ID id, bool throwIfWrongType) { |
| 144 |
|
✗ |
shared_ptr<T> comp; |
| 145 |
|
|
try { |
| 146 |
|
✗ |
comp = HPP_DYNAMIC_PTR_CAST(T, graph()->get(id).lock()); |
| 147 |
|
✗ |
} catch (std::out_of_range& e) { |
| 148 |
|
✗ |
throw Error(e.what()); |
| 149 |
|
|
} |
| 150 |
|
✗ |
if (throwIfWrongType && !comp) { |
| 151 |
|
✗ |
std::stringstream ss; |
| 152 |
|
✗ |
ss << "ID " << id << " is not a " << toStr<T>(); |
| 153 |
|
✗ |
throw Error(ss.str().c_str()); |
| 154 |
|
|
} |
| 155 |
|
✗ |
return comp; |
| 156 |
|
|
} |
| 157 |
|
|
|
| 158 |
|
✗ |
Long Graph::createGraph(const char* graphName) { |
| 159 |
|
|
try { |
| 160 |
|
✗ |
std::string name(graphName); |
| 161 |
|
✗ |
if (problemSolver()->graphs.has(name)) { |
| 162 |
|
✗ |
HPP_THROW(Error, "A graph named " << name << " already exists"); |
| 163 |
|
|
} |
| 164 |
|
|
|
| 165 |
|
✗ |
DevicePtr_t robot = problemSolver()->robot(); |
| 166 |
|
✗ |
if (!robot) throw Error("Build the robot first."); |
| 167 |
|
|
// Create default steering method to store in edges, until we define a |
| 168 |
|
|
// factory for steering methods. |
| 169 |
|
|
graph::GraphPtr_t g = |
| 170 |
|
✗ |
graph::Graph::create(name, robot, problemSolver()->problem()); |
| 171 |
|
✗ |
g->maxIterations(problemSolver()->maxIterProjection()); |
| 172 |
|
✗ |
g->errorThreshold(problemSolver()->errorThreshold()); |
| 173 |
|
|
|
| 174 |
|
✗ |
problemSolver()->graphs.add(name, g); |
| 175 |
|
✗ |
problemSolver()->constraintGraph(name); |
| 176 |
|
✗ |
return (Long)g->id(); |
| 177 |
|
✗ |
} catch (std::exception& e) { |
| 178 |
|
✗ |
throw Error(e.what()); |
| 179 |
|
|
} |
| 180 |
|
|
} |
| 181 |
|
|
|
| 182 |
|
✗ |
void Graph::deleteGraph(const char* graphName) { |
| 183 |
|
|
try { |
| 184 |
|
✗ |
std::string name(graphName); |
| 185 |
|
✗ |
if (!problemSolver()->graphs.has(name)) { |
| 186 |
|
✗ |
HPP_THROW(Error, "There is no graph named " << name << "."); |
| 187 |
|
|
} |
| 188 |
|
✗ |
problemSolver()->graphs.erase(name); |
| 189 |
|
✗ |
} catch (std::exception& e) { |
| 190 |
|
✗ |
throw Error(e.what()); |
| 191 |
|
|
} |
| 192 |
|
|
} |
| 193 |
|
|
|
| 194 |
|
✗ |
void Graph::selectGraph(const char* graphName) { |
| 195 |
|
|
try { |
| 196 |
|
✗ |
problemSolver()->constraintGraph(graphName); |
| 197 |
|
✗ |
} catch (const std::exception& e) { |
| 198 |
|
✗ |
throw Error(e.what()); |
| 199 |
|
|
} |
| 200 |
|
|
} |
| 201 |
|
|
|
| 202 |
|
✗ |
void Graph::createSubGraph(const char* subgraphName) { |
| 203 |
|
|
try { |
| 204 |
|
|
graph::GuidedStateSelectorPtr_t ns = graph::GuidedStateSelector::create( |
| 205 |
|
✗ |
subgraphName, problemSolver()->roadmap()); |
| 206 |
|
✗ |
graph()->stateSelector(ns); |
| 207 |
|
✗ |
} catch (const std::exception& exc) { |
| 208 |
|
✗ |
throw(Error(exc.what())); |
| 209 |
|
|
} |
| 210 |
|
|
} |
| 211 |
|
|
|
| 212 |
|
✗ |
void Graph::setTargetNodeList(const ID graphId, const hpp::IDseq& nodes) { |
| 213 |
|
✗ |
graph::GraphPtr_t graph = getComp<graph::Graph>(graphId); |
| 214 |
|
|
graph::GuidedStateSelectorPtr_t ns = |
| 215 |
|
✗ |
HPP_DYNAMIC_PTR_CAST(graph::GuidedStateSelector, graph->stateSelector()); |
| 216 |
|
✗ |
if (!ns) |
| 217 |
|
✗ |
throw Error("The state selector is not of type GuidedStateSelector."); |
| 218 |
|
|
try { |
| 219 |
|
✗ |
graph::States_t nl; |
| 220 |
|
✗ |
for (unsigned int i = 0; i < nodes.length(); ++i) |
| 221 |
|
✗ |
nl.push_back(getComp<graph::State>(nodes[i])); |
| 222 |
|
✗ |
ns->setStateList(nl); |
| 223 |
|
✗ |
} catch (std::out_of_range& e) { |
| 224 |
|
✗ |
throw Error(e.what()); |
| 225 |
|
|
} |
| 226 |
|
|
} |
| 227 |
|
|
|
| 228 |
|
✗ |
Long Graph::createNode(const Long graphId, const char* nodeName, |
| 229 |
|
|
const bool waypoint, const Long priority) { |
| 230 |
|
✗ |
graph::GraphPtr_t graph = getComp<graph::Graph>(graphId); |
| 231 |
|
✗ |
if (graph->stateSelector()) { |
| 232 |
|
|
graph::StatePtr_t state = |
| 233 |
|
✗ |
graph->stateSelector()->createState(nodeName, waypoint, priority); |
| 234 |
|
✗ |
return (Long)state->id(); |
| 235 |
|
✗ |
} else { |
| 236 |
|
✗ |
throw Error("Graph has no state selector."); |
| 237 |
|
|
} |
| 238 |
|
|
} |
| 239 |
|
|
|
| 240 |
|
✗ |
Long Graph::createEdge(const Long nodeFromId, const Long nodeToId, |
| 241 |
|
|
const char* edgeName, const Long w, |
| 242 |
|
|
const Long isInNodeId) { |
| 243 |
|
✗ |
graph::StatePtr_t from = getComp<graph::State>(nodeFromId), |
| 244 |
|
✗ |
to = getComp<graph::State>(nodeToId), |
| 245 |
|
✗ |
isInState = getComp<graph::State>(isInNodeId); |
| 246 |
|
|
|
| 247 |
|
✗ |
EdgePtr_t edge = from->linkTo(edgeName, to, (size_type)w, |
| 248 |
|
✗ |
(graph::State::EdgeFactory)Edge::create); |
| 249 |
|
✗ |
edge->state(isInState); |
| 250 |
|
|
|
| 251 |
|
✗ |
return (Long)edge->id(); |
| 252 |
|
|
} |
| 253 |
|
|
|
| 254 |
|
✗ |
Long Graph::createWaypointEdge(const Long nodeFromId, const Long nodeToId, |
| 255 |
|
|
const char* edgeName, const Long nb, |
| 256 |
|
|
const Long w, const Long isInNodeId) { |
| 257 |
|
|
try { |
| 258 |
|
✗ |
graph::StatePtr_t from = getComp<graph::State>(nodeFromId), |
| 259 |
|
✗ |
to = getComp<graph::State>(nodeToId), |
| 260 |
|
✗ |
isInNode = getComp<graph::State>(isInNodeId); |
| 261 |
|
|
|
| 262 |
|
|
EdgePtr_t edge_pc = |
| 263 |
|
✗ |
from->linkTo(edgeName, to, (size_type)w, |
| 264 |
|
✗ |
(graph::State::EdgeFactory)WaypointEdge::create); |
| 265 |
|
|
|
| 266 |
|
✗ |
edge_pc->state(isInNode); |
| 267 |
|
✗ |
WaypointEdgePtr_t edge = HPP_DYNAMIC_PTR_CAST(WaypointEdge, edge_pc); |
| 268 |
|
|
|
| 269 |
|
✗ |
edge->nbWaypoints(nb); |
| 270 |
|
✗ |
return (Long)edge->id(); |
| 271 |
|
✗ |
} catch (const std::exception& exc) { |
| 272 |
|
✗ |
throw(Error(exc.what())); |
| 273 |
|
|
} |
| 274 |
|
|
} |
| 275 |
|
|
|
| 276 |
|
✗ |
void Graph::setWaypoint(const ID waypointEdgeId, const Long index, |
| 277 |
|
|
const ID edgeId, const ID nodeId) { |
| 278 |
|
|
try { |
| 279 |
|
✗ |
WaypointEdgePtr_t we = getComp<graph::WaypointEdge>(waypointEdgeId); |
| 280 |
|
✗ |
EdgePtr_t edge = getComp<Edge>(edgeId); |
| 281 |
|
✗ |
graph::StatePtr_t state = getComp<graph::State>(nodeId); |
| 282 |
|
|
|
| 283 |
|
✗ |
if (index < 0 || (std::size_t)index > we->nbWaypoints()) |
| 284 |
|
✗ |
throw Error("Invalid index"); |
| 285 |
|
✗ |
we->setWaypoint(index, edge, state); |
| 286 |
|
✗ |
} catch (const std::exception& exc) { |
| 287 |
|
✗ |
throw(Error(exc.what())); |
| 288 |
|
|
} |
| 289 |
|
|
} |
| 290 |
|
|
|
| 291 |
|
✗ |
void Graph::getGraph(GraphComp_out graph_out, GraphElements_out elmts) { |
| 292 |
|
✗ |
GraphComps_t comp_n, comp_e; |
| 293 |
|
✗ |
GraphComp comp_g, current; |
| 294 |
|
|
|
| 295 |
|
✗ |
graph::StatePtr_t n; |
| 296 |
|
✗ |
graph::EdgePtr_t e; |
| 297 |
|
|
|
| 298 |
|
✗ |
ULong len_edges = 0; |
| 299 |
|
✗ |
ULong len_nodes = 0; |
| 300 |
|
|
try { |
| 301 |
|
✗ |
graph::GraphPtr_t g = graph(); |
| 302 |
|
|
|
| 303 |
|
|
// Set the graph values |
| 304 |
|
✗ |
graph_out = new GraphComp(); |
| 305 |
|
✗ |
graph_out->name = g->name().c_str(); |
| 306 |
|
✗ |
graph_out->id = (Long)g->id(); |
| 307 |
|
|
|
| 308 |
|
✗ |
for (std::size_t i = 0; i < g->nbComponents(); ++i) { |
| 309 |
|
✗ |
if (i == g->id()) continue; |
| 310 |
|
✗ |
graph::GraphComponentPtr_t gcomponent = g->get(i).lock(); |
| 311 |
|
✗ |
if (!gcomponent) continue; |
| 312 |
|
✗ |
current.name = gcomponent->name().c_str(); |
| 313 |
|
✗ |
current.id = (Long)gcomponent->id(); |
| 314 |
|
✗ |
n = HPP_DYNAMIC_PTR_CAST(graph::State, gcomponent); |
| 315 |
|
✗ |
e = HPP_DYNAMIC_PTR_CAST(graph::Edge, gcomponent); |
| 316 |
|
✗ |
if (n) { |
| 317 |
|
✗ |
comp_n.length(len_nodes + 1); |
| 318 |
|
✗ |
comp_n[len_nodes] = current; |
| 319 |
|
✗ |
len_nodes++; |
| 320 |
|
✗ |
} else if (e) { |
| 321 |
|
✗ |
comp_e.length(len_edges + 1); |
| 322 |
|
|
graph::WaypointEdgePtr_t we = |
| 323 |
|
✗ |
HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, e); |
| 324 |
|
✗ |
if (we) { |
| 325 |
|
✗ |
current.waypoints.length((ULong)we->nbWaypoints()); |
| 326 |
|
✗ |
for (std::size_t i = 0; i < we->nbWaypoints(); ++i) |
| 327 |
|
✗ |
current.waypoints[(ULong)i] = (ID)we->waypoint(i)->stateTo()->id(); |
| 328 |
|
|
} else { |
| 329 |
|
✗ |
current.waypoints.length(0); |
| 330 |
|
|
} |
| 331 |
|
✗ |
current.start = (Long)e->stateFrom()->id(); |
| 332 |
|
✗ |
current.end = (Long)e->stateTo()->id(); |
| 333 |
|
✗ |
comp_e[len_edges] = current; |
| 334 |
|
✗ |
len_edges++; |
| 335 |
|
|
} |
| 336 |
|
|
} |
| 337 |
|
✗ |
elmts = new GraphElements; |
| 338 |
|
✗ |
elmts->nodes = comp_n; |
| 339 |
|
✗ |
elmts->edges = comp_e; |
| 340 |
|
✗ |
} catch (std::out_of_range& e) { |
| 341 |
|
✗ |
throw Error(e.what()); |
| 342 |
|
|
} |
| 343 |
|
|
} |
| 344 |
|
|
|
| 345 |
|
✗ |
void Graph::getEdgeStat(ID edgeId, Names_t_out reasons, intSeq_out freqs) { |
| 346 |
|
|
try { |
| 347 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId, true); |
| 348 |
|
✗ |
core::PathPlannerPtr_t p = problemSolver()->pathPlanner(); |
| 349 |
|
✗ |
if (!p) throw Error("There is no planner"); |
| 350 |
|
✗ |
ManipulationPlannerPtr_t mp = HPP_DYNAMIC_PTR_CAST(ManipulationPlanner, p); |
| 351 |
|
✗ |
if (!mp) throw Error("The planner must be a ManipulationPlanner"); |
| 352 |
|
|
|
| 353 |
|
✗ |
StringList_t errors = ManipulationPlanner::errorList(); |
| 354 |
|
✗ |
ManipulationPlanner::ErrorFreqs_t fes = mp->getEdgeStat(edge); |
| 355 |
|
|
|
| 356 |
|
✗ |
Names_t* r_ptr = toNames_t(errors.begin(), errors.end()); |
| 357 |
|
✗ |
intSeq* f_ptr = toIntSeq(fes.begin(), fes.end()); |
| 358 |
|
|
|
| 359 |
|
✗ |
reasons = r_ptr; |
| 360 |
|
✗ |
freqs = f_ptr; |
| 361 |
|
✗ |
} catch (std::out_of_range& e) { |
| 362 |
|
✗ |
throw Error(e.what()); |
| 363 |
|
|
} |
| 364 |
|
|
} |
| 365 |
|
|
|
| 366 |
|
✗ |
Long Graph::getFrequencyOfNodeInRoadmap(ID nodeId, |
| 367 |
|
|
intSeq_out freqPerConnectedComponent) { |
| 368 |
|
|
try { |
| 369 |
|
✗ |
graph::StatePtr_t state = getComp<graph::State>(nodeId, true); |
| 370 |
|
|
// Long nb = graph_->nodeHistogram()->freq(graph::NodeBin(node)); |
| 371 |
|
✗ |
std::size_t nb = 0; |
| 372 |
|
|
const core::ConnectedComponents_t& ccs = |
| 373 |
|
✗ |
problemSolver()->roadmap()->connectedComponents(); |
| 374 |
|
✗ |
core::ConnectedComponents_t::const_iterator _cc; |
| 375 |
|
✗ |
std::vector<std::size_t> freqs; |
| 376 |
|
✗ |
for (_cc = ccs.begin(); _cc != ccs.end(); ++_cc) { |
| 377 |
|
|
manipulation::ConnectedComponentPtr_t cc = |
| 378 |
|
✗ |
HPP_DYNAMIC_PTR_CAST(manipulation::ConnectedComponent, *_cc); |
| 379 |
|
✗ |
if (!cc) throw Error("Connected component is not of the right type."); |
| 380 |
|
✗ |
freqs.push_back(cc->getRoadmapNodes(state).size()); |
| 381 |
|
✗ |
nb += freqs.back(); |
| 382 |
|
|
} |
| 383 |
|
✗ |
freqPerConnectedComponent = toIntSeq(freqs.begin(), freqs.end()); |
| 384 |
|
✗ |
return (Long)nb; |
| 385 |
|
✗ |
} catch (std::out_of_range& e) { |
| 386 |
|
✗ |
throw Error(e.what()); |
| 387 |
|
|
} |
| 388 |
|
|
} |
| 389 |
|
|
|
| 390 |
|
✗ |
bool Graph::getConfigProjectorStats(ID elmt, ConfigProjStat_out config, |
| 391 |
|
|
ConfigProjStat_out path) { |
| 392 |
|
|
try { |
| 393 |
|
✗ |
graph::StatePtr_t state = getComp<graph::State>(elmt, false); |
| 394 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(elmt, false); |
| 395 |
|
✗ |
if (state) { |
| 396 |
|
|
ConfigProjectorPtr_t proj = |
| 397 |
|
✗ |
graph()->configConstraint(state)->configProjector(); |
| 398 |
|
✗ |
if (proj) { |
| 399 |
|
✗ |
config.success = (Long)proj->statistics().nbSuccess(); |
| 400 |
|
✗ |
config.error = (Long)proj->statistics().nbFailure(); |
| 401 |
|
✗ |
config.nbObs = (Long)proj->statistics().numberOfObservations(); |
| 402 |
|
|
} |
| 403 |
|
✗ |
path.success = 0; |
| 404 |
|
✗ |
path.error = 0; |
| 405 |
|
✗ |
path.nbObs = 0; |
| 406 |
|
✗ |
return true; |
| 407 |
|
✗ |
} else if (edge) { |
| 408 |
|
|
ConfigProjectorPtr_t proj = |
| 409 |
|
✗ |
graph()->targetConstraint(edge)->configProjector(); |
| 410 |
|
✗ |
if (proj) { |
| 411 |
|
✗ |
config.success = (Long)proj->statistics().nbSuccess(); |
| 412 |
|
✗ |
config.error = (Long)proj->statistics().nbFailure(); |
| 413 |
|
✗ |
config.nbObs = (Long)proj->statistics().numberOfObservations(); |
| 414 |
|
|
} |
| 415 |
|
✗ |
proj = graph()->pathConstraint(edge)->configProjector(); |
| 416 |
|
✗ |
if (proj) { |
| 417 |
|
✗ |
path.success = (Long)proj->statistics().nbSuccess(); |
| 418 |
|
✗ |
path.error = (Long)proj->statistics().nbFailure(); |
| 419 |
|
✗ |
path.nbObs = (Long)proj->statistics().numberOfObservations(); |
| 420 |
|
|
} |
| 421 |
|
✗ |
return true; |
| 422 |
|
✗ |
} else { |
| 423 |
|
✗ |
throw Error("The ID does not exist."); |
| 424 |
|
|
} |
| 425 |
|
|
return false; |
| 426 |
|
✗ |
} catch (std::out_of_range& e) { |
| 427 |
|
✗ |
throw Error(e.what()); |
| 428 |
|
|
} |
| 429 |
|
|
} |
| 430 |
|
|
|
| 431 |
|
✗ |
Long Graph::getWaypoint(const Long edgeId, const Long index, |
| 432 |
|
|
hpp::ID_out nodeId) { |
| 433 |
|
|
try { |
| 434 |
|
✗ |
graph::WaypointEdgePtr_t edge = getComp<graph::WaypointEdge>(edgeId); |
| 435 |
|
|
|
| 436 |
|
✗ |
if (index < 0 || (std::size_t)index > edge->nbWaypoints()) |
| 437 |
|
✗ |
throw Error("Invalid index"); |
| 438 |
|
✗ |
graph::EdgePtr_t waypoint = edge->waypoint(index); |
| 439 |
|
✗ |
nodeId = (Long)waypoint->stateTo()->id(); |
| 440 |
|
✗ |
return (Long)waypoint->id(); |
| 441 |
|
✗ |
} catch (std::out_of_range& e) { |
| 442 |
|
✗ |
throw Error(e.what()); |
| 443 |
|
|
} |
| 444 |
|
|
} |
| 445 |
|
|
|
| 446 |
|
✗ |
Long Graph::createLevelSetEdge(const Long nodeFromId, const Long nodeToId, |
| 447 |
|
|
const char* edgeName, const Long w, |
| 448 |
|
|
const ID isInNodeId) { |
| 449 |
|
|
try { |
| 450 |
|
✗ |
graph::StatePtr_t from = getComp<graph::State>(nodeFromId), |
| 451 |
|
✗ |
to = getComp<graph::State>(nodeToId), |
| 452 |
|
✗ |
isInState = getComp<graph::State>(isInNodeId); |
| 453 |
|
|
|
| 454 |
|
|
graph::EdgePtr_t edge = |
| 455 |
|
✗ |
from->linkTo(edgeName, to, (size_type)w, |
| 456 |
|
✗ |
(graph::State::EdgeFactory)graph::LevelSetEdge::create); |
| 457 |
|
|
|
| 458 |
|
✗ |
edge->state(isInState); |
| 459 |
|
|
|
| 460 |
|
✗ |
return (Long)edge->id(); |
| 461 |
|
✗ |
} catch (std::out_of_range& e) { |
| 462 |
|
✗ |
throw Error(e.what()); |
| 463 |
|
|
} |
| 464 |
|
|
} |
| 465 |
|
|
|
| 466 |
|
✗ |
void Graph::addLevelSetFoliation(const Long edgeId, const hpp::Names_t& condNC, |
| 467 |
|
|
const hpp::Names_t& paramNC) { |
| 468 |
|
|
try { |
| 469 |
|
✗ |
graph::LevelSetEdgePtr_t edge = getComp<graph::LevelSetEdge>(edgeId); |
| 470 |
|
✗ |
for (CORBA::ULong i = 0; i < condNC.length(); ++i) { |
| 471 |
|
✗ |
std::string name(condNC[i]); |
| 472 |
|
✗ |
edge->insertConditionConstraint( |
| 473 |
|
✗ |
problemSolver()->numericalConstraints.get(name)->copy()); |
| 474 |
|
|
} |
| 475 |
|
✗ |
for (CORBA::ULong i = 0; i < paramNC.length(); ++i) { |
| 476 |
|
✗ |
std::string name(paramNC[i]); |
| 477 |
|
✗ |
edge->insertParamConstraint( |
| 478 |
|
✗ |
problemSolver()->numericalConstraints.get(name)->copy()); |
| 479 |
|
|
} |
| 480 |
|
✗ |
} catch (std::exception& err) { |
| 481 |
|
✗ |
throw Error(err.what()); |
| 482 |
|
|
} |
| 483 |
|
|
} |
| 484 |
|
|
|
| 485 |
|
✗ |
void Graph::setContainingNode(const ID edgeId, const ID nodeId) { |
| 486 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 487 |
|
✗ |
graph::StatePtr_t state = getComp<graph::State>(nodeId); |
| 488 |
|
|
try { |
| 489 |
|
✗ |
edge->state(state); |
| 490 |
|
|
} catch (std::exception& err) { |
| 491 |
|
|
throw Error(err.what()); |
| 492 |
|
|
} |
| 493 |
|
|
} |
| 494 |
|
|
|
| 495 |
|
✗ |
char* Graph::getContainingNode(const ID edgeId) { |
| 496 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 497 |
|
|
try { |
| 498 |
|
✗ |
std::string name(edge->state()->name()); |
| 499 |
|
✗ |
char* res = new char[name.size() + 1]; |
| 500 |
|
✗ |
strcpy(res, name.c_str()); |
| 501 |
|
✗ |
return res; |
| 502 |
|
✗ |
} catch (std::exception& err) { |
| 503 |
|
✗ |
throw Error(err.what()); |
| 504 |
|
|
} |
| 505 |
|
|
} |
| 506 |
|
|
|
| 507 |
|
✗ |
void Graph::addNumericalConstraints(const Long graphComponentId, |
| 508 |
|
|
const hpp::Names_t& constraintNames) { |
| 509 |
|
|
graph::GraphComponentPtr_t component = |
| 510 |
|
✗ |
getComp<graph::GraphComponent>(graphComponentId, true); |
| 511 |
|
|
|
| 512 |
|
✗ |
if (constraintNames.length() > 0) { |
| 513 |
|
|
try { |
| 514 |
|
✗ |
for (CORBA::ULong i = 0; i < constraintNames.length(); ++i) { |
| 515 |
|
✗ |
std::string name(constraintNames[i]); |
| 516 |
|
✗ |
if (!problemSolver()->numericalConstraint(name)) { |
| 517 |
|
✗ |
std::ostringstream os; |
| 518 |
|
✗ |
os << "Numerical constraint \"" << name << "\" does not exist."; |
| 519 |
|
✗ |
throw Error(os.str().c_str()); |
| 520 |
|
|
} |
| 521 |
|
✗ |
component->addNumericalConstraint( |
| 522 |
|
✗ |
problemSolver()->numericalConstraint(name)); |
| 523 |
|
|
} |
| 524 |
|
✗ |
} catch (std::exception& err) { |
| 525 |
|
✗ |
throw Error(err.what()); |
| 526 |
|
|
} |
| 527 |
|
|
} |
| 528 |
|
|
} |
| 529 |
|
|
|
| 530 |
|
✗ |
void Graph::getNumericalConstraints(const Long graphComponentId, |
| 531 |
|
|
hpp::Names_t_out names) { |
| 532 |
|
|
graph::GraphComponentPtr_t elmt = |
| 533 |
|
✗ |
getComp<graph::GraphComponent>(graphComponentId); |
| 534 |
|
✗ |
core::NumericalConstraints_t constraints = elmt->numericalConstraints(); |
| 535 |
|
✗ |
names = new hpp::Names_t; |
| 536 |
|
✗ |
names->length((ULong)constraints.size()); |
| 537 |
|
✗ |
int i = 0; |
| 538 |
|
✗ |
for (core::NumericalConstraints_t::iterator it = constraints.begin(); |
| 539 |
|
✗ |
it != constraints.end(); ++it) { |
| 540 |
|
✗ |
(*names)[i] = (*it)->function().name().c_str(); |
| 541 |
|
✗ |
i++; |
| 542 |
|
|
} |
| 543 |
|
|
} |
| 544 |
|
|
|
| 545 |
|
✗ |
void Graph::resetConstraints(const Long graphComponentId) { |
| 546 |
|
|
graph::GraphComponentPtr_t component = |
| 547 |
|
✗ |
getComp<graph::GraphComponent>(graphComponentId, true); |
| 548 |
|
✗ |
component->resetNumericalConstraints(); |
| 549 |
|
|
} |
| 550 |
|
|
|
| 551 |
|
✗ |
void Graph::addNumericalConstraintsForPath( |
| 552 |
|
|
const Long nodeId, const hpp::Names_t& constraintNames) { |
| 553 |
|
✗ |
graph::StatePtr_t n = getComp<graph::State>(nodeId); |
| 554 |
|
|
|
| 555 |
|
✗ |
if (constraintNames.length() > 0) { |
| 556 |
|
|
try { |
| 557 |
|
✗ |
for (CORBA::ULong i = 0; i < constraintNames.length(); ++i) { |
| 558 |
|
✗ |
std::string name(constraintNames[i]); |
| 559 |
|
✗ |
n->addNumericalConstraintForPath( |
| 560 |
|
✗ |
problemSolver()->numericalConstraint(name)->copy()); |
| 561 |
|
|
} |
| 562 |
|
✗ |
} catch (std::exception& err) { |
| 563 |
|
✗ |
throw Error(err.what()); |
| 564 |
|
|
} |
| 565 |
|
|
} |
| 566 |
|
|
} |
| 567 |
|
|
|
| 568 |
|
✗ |
void Graph::removeCollisionPairFromEdge(ID edgeId, const char* joint1, |
| 569 |
|
|
const char* joint2) { |
| 570 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 571 |
|
|
|
| 572 |
|
|
try { |
| 573 |
|
|
using hpp::core::RelativeMotion; |
| 574 |
|
✗ |
RelativeMotion::matrix_type m(edge->relativeMotion()); |
| 575 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
| 576 |
|
✗ |
JointIndex i1 = robot->getJointByName(joint1)->index(); |
| 577 |
|
✗ |
JointIndex i2 = robot->getJointByName(joint2)->index(); |
| 578 |
|
✗ |
m(i1, i2) = m(i2, i1) = RelativeMotion::Constrained; |
| 579 |
|
✗ |
edge->relativeMotion(m); |
| 580 |
|
✗ |
} catch (std::exception& err) { |
| 581 |
|
✗ |
throw Error(err.what()); |
| 582 |
|
|
} |
| 583 |
|
|
} |
| 584 |
|
|
|
| 585 |
|
✗ |
void Graph::getNode(const hpp::floatSeq& dofArray, ID_out output) { |
| 586 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
| 587 |
|
|
try { |
| 588 |
|
✗ |
Configuration_t config(floatSeqToConfig(robot, dofArray, true)); |
| 589 |
|
✗ |
graph::StatePtr_t state = graph()->getState(config); |
| 590 |
|
✗ |
output = (Long)state->id(); |
| 591 |
|
✗ |
} catch (std::exception& e) { |
| 592 |
|
✗ |
throw Error(e.what()); |
| 593 |
|
|
} |
| 594 |
|
|
} |
| 595 |
|
|
|
| 596 |
|
✗ |
bool Graph::applyNodeConstraints(hpp::ID id, const hpp::floatSeq& input, |
| 597 |
|
|
hpp::floatSeq_out output, |
| 598 |
|
|
double& residualError) { |
| 599 |
|
|
/// First get the constraint. |
| 600 |
|
✗ |
ConstraintSetPtr_t constraint; |
| 601 |
|
|
try { |
| 602 |
|
✗ |
graph::GraphComponentPtr_t comp = graph()->get((size_t)id).lock(); |
| 603 |
|
✗ |
graph::EdgePtr_t edge = HPP_DYNAMIC_PTR_CAST(graph::Edge, comp); |
| 604 |
|
✗ |
graph::StatePtr_t state = HPP_DYNAMIC_PTR_CAST(graph::State, comp); |
| 605 |
|
✗ |
if (edge) { |
| 606 |
|
✗ |
constraint = graph(false)->targetConstraint(edge); |
| 607 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
| 608 |
|
✗ |
if (core::ConfigProjectorPtr_t cp = constraint->configProjector()) { |
| 609 |
|
✗ |
cp->rightHandSideFromConfig(robot->currentConfiguration()); |
| 610 |
|
|
} |
| 611 |
|
✗ |
} else if (state) |
| 612 |
|
✗ |
constraint = graph(false)->configConstraint(state); |
| 613 |
|
|
else { |
| 614 |
|
✗ |
std::stringstream ss; |
| 615 |
|
✗ |
ss << "ID " << id << " is neither an edge nor a state"; |
| 616 |
|
✗ |
std::string errmsg = ss.str(); |
| 617 |
|
✗ |
throw Error(errmsg.c_str()); |
| 618 |
|
|
} |
| 619 |
|
✗ |
bool success = false; |
| 620 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
| 621 |
|
✗ |
Configuration_t config = floatSeqToConfig(robot, input, true); |
| 622 |
|
✗ |
success = constraint->apply(config); |
| 623 |
|
✗ |
if (hpp::core::ConfigProjectorPtr_t configProjector = |
| 624 |
|
✗ |
constraint->configProjector()) { |
| 625 |
|
✗ |
residualError = configProjector->residualError(); |
| 626 |
|
|
} |
| 627 |
|
✗ |
output = vectorToFloatSeq(config); |
| 628 |
|
✗ |
return success; |
| 629 |
|
✗ |
} catch (const std::exception& exc) { |
| 630 |
|
✗ |
throw hpp::Error(exc.what()); |
| 631 |
|
|
} |
| 632 |
|
|
} |
| 633 |
|
|
|
| 634 |
|
✗ |
bool Graph::applyEdgeLeafConstraints(hpp::ID IDedge, const hpp::floatSeq& qleaf, |
| 635 |
|
|
const hpp::floatSeq& input, |
| 636 |
|
|
hpp::floatSeq_out output, |
| 637 |
|
|
double& residualError) { |
| 638 |
|
|
/// First get the constraint. |
| 639 |
|
✗ |
graph::EdgePtr_t edge; |
| 640 |
|
|
try { |
| 641 |
|
|
edge = |
| 642 |
|
✗ |
HPP_DYNAMIC_PTR_CAST(graph::Edge, graph()->get((size_t)IDedge).lock()); |
| 643 |
|
✗ |
if (!edge) { |
| 644 |
|
✗ |
std::stringstream ss; |
| 645 |
|
✗ |
ss << "ID " << IDedge << " is not an edge"; |
| 646 |
|
✗ |
std::string errmsg = ss.str(); |
| 647 |
|
✗ |
throw Error(errmsg.c_str()); |
| 648 |
|
|
} |
| 649 |
|
✗ |
bool success = false; |
| 650 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
| 651 |
|
✗ |
Configuration_t config = floatSeqToConfig(robot, input, true); |
| 652 |
|
✗ |
Configuration_t qRhs = floatSeqToConfig(robot, qleaf, true); |
| 653 |
|
✗ |
ConstraintSetPtr_t cs(edge->pathConstraint()); |
| 654 |
|
✗ |
assert(cs); |
| 655 |
|
|
|
| 656 |
|
✗ |
if (cs->configProjector()) { |
| 657 |
|
✗ |
cs->configProjector()->rightHandSideFromConfig(qRhs); |
| 658 |
|
✗ |
success = cs->apply(config); |
| 659 |
|
✗ |
residualError = cs->configProjector()->residualError(); |
| 660 |
|
|
} else { |
| 661 |
|
✗ |
residualError = 0; |
| 662 |
|
|
} |
| 663 |
|
|
|
| 664 |
|
✗ |
output = vectorToFloatSeq(config); |
| 665 |
|
✗ |
return success; |
| 666 |
|
✗ |
} catch (const std::exception& exc) { |
| 667 |
|
✗ |
throw hpp::Error(exc.what()); |
| 668 |
|
|
} |
| 669 |
|
|
} |
| 670 |
|
|
|
| 671 |
|
✗ |
bool Graph::generateTargetConfig(hpp::ID IDedge, const hpp::floatSeq& qleaf, |
| 672 |
|
|
const hpp::floatSeq& input, |
| 673 |
|
|
hpp::floatSeq_out output, |
| 674 |
|
|
double& residualError) { |
| 675 |
|
|
/// First get the constraint. |
| 676 |
|
✗ |
graph::EdgePtr_t edge; |
| 677 |
|
|
try { |
| 678 |
|
|
edge = |
| 679 |
|
✗ |
HPP_DYNAMIC_PTR_CAST(graph::Edge, graph()->get((size_t)IDedge).lock()); |
| 680 |
|
✗ |
if (!edge) { |
| 681 |
|
✗ |
std::stringstream ss; |
| 682 |
|
✗ |
ss << "ID " << IDedge << " is not an edge"; |
| 683 |
|
✗ |
std::string errmsg = ss.str(); |
| 684 |
|
✗ |
throw Error(errmsg.c_str()); |
| 685 |
|
|
} |
| 686 |
|
✗ |
bool success = false; |
| 687 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
| 688 |
|
✗ |
Configuration_t config = floatSeqToConfig(robot, input, true); |
| 689 |
|
✗ |
Configuration_t qRhs = floatSeqToConfig(robot, qleaf, true); |
| 690 |
|
✗ |
value_type dist = 0; |
| 691 |
|
✗ |
core::NodePtr_t nNode = problemSolver()->roadmap()->nearestNode(qRhs, dist); |
| 692 |
|
✗ |
if (dist < 1e-8) |
| 693 |
|
✗ |
success = edge->generateTargetConfig(nNode, config); |
| 694 |
|
|
else |
| 695 |
|
✗ |
success = edge->generateTargetConfig(qRhs, config); |
| 696 |
|
|
|
| 697 |
|
|
hpp::core::ConfigProjectorPtr_t configProjector( |
| 698 |
|
✗ |
edge->targetConstraint()->configProjector()); |
| 699 |
|
✗ |
if (configProjector) { |
| 700 |
|
✗ |
residualError = configProjector->residualError(); |
| 701 |
|
|
} else { |
| 702 |
|
|
hppDout(info, "No config projector."); |
| 703 |
|
|
} |
| 704 |
|
✗ |
ULong size = (ULong)config.size(); |
| 705 |
|
✗ |
hpp::floatSeq* q_ptr = new hpp::floatSeq(); |
| 706 |
|
✗ |
q_ptr->length(size); |
| 707 |
|
|
|
| 708 |
|
✗ |
for (std::size_t i = 0; i < size; ++i) { |
| 709 |
|
✗ |
(*q_ptr)[(ULong)i] = config[i]; |
| 710 |
|
|
} |
| 711 |
|
✗ |
output = q_ptr; |
| 712 |
|
✗ |
return success; |
| 713 |
|
✗ |
} catch (const std::exception& exc) { |
| 714 |
|
✗ |
throw hpp::Error(exc.what()); |
| 715 |
|
|
} |
| 716 |
|
|
} |
| 717 |
|
|
|
| 718 |
|
✗ |
CORBA::Boolean Graph::getConfigErrorForNode(ID nodeId, |
| 719 |
|
|
const hpp::floatSeq& dofArray, |
| 720 |
|
|
hpp::floatSeq_out error) { |
| 721 |
|
✗ |
graph::StatePtr_t state = getComp<graph::State>(nodeId); |
| 722 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
| 723 |
|
|
try { |
| 724 |
|
✗ |
vector_t err; |
| 725 |
|
✗ |
Configuration_t config(floatSeqToConfig(robot, dofArray, true)); |
| 726 |
|
✗ |
bool res = graph()->getConfigErrorForState(config, state, err); |
| 727 |
|
✗ |
error = vectorToFloatSeq(err); |
| 728 |
|
✗ |
return res; |
| 729 |
|
✗ |
} catch (const std::exception& exc) { |
| 730 |
|
✗ |
throw Error(exc.what()); |
| 731 |
|
|
} |
| 732 |
|
|
} |
| 733 |
|
|
|
| 734 |
|
✗ |
CORBA::Boolean Graph::getConfigErrorForEdge(ID edgeId, |
| 735 |
|
|
const hpp::floatSeq& dofArray, |
| 736 |
|
|
hpp::floatSeq_out error) { |
| 737 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
| 738 |
|
|
try { |
| 739 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 740 |
|
|
// If steering method is not completely set in the graph, create |
| 741 |
|
|
// one. |
| 742 |
|
✗ |
if (!edge->parentGraph()->problem()->manipulationSteeringMethod() || |
| 743 |
|
✗ |
!edge->parentGraph() |
| 744 |
|
✗ |
->problem() |
| 745 |
|
✗ |
->manipulationSteeringMethod() |
| 746 |
|
✗ |
->innerSteeringMethod()) { |
| 747 |
|
✗ |
problemSolver()->initSteeringMethod(); |
| 748 |
|
✗ |
if (!edge->parentGraph()->problem()->manipulationSteeringMethod() || |
| 749 |
|
✗ |
!edge->parentGraph() |
| 750 |
|
✗ |
->problem() |
| 751 |
|
✗ |
->manipulationSteeringMethod() |
| 752 |
|
✗ |
->innerSteeringMethod()) |
| 753 |
|
✗ |
throw Error("Could not initialize the steering method."); |
| 754 |
|
|
} |
| 755 |
|
✗ |
vector_t err; |
| 756 |
|
✗ |
Configuration_t config(floatSeqToConfig(robot, dofArray, true)); |
| 757 |
|
✗ |
bool res = graph()->getConfigErrorForEdge(config, edge, err); |
| 758 |
|
✗ |
floatSeq* e = new floatSeq(); |
| 759 |
|
✗ |
e->length((ULong)err.size()); |
| 760 |
|
✗ |
for (std::size_t i = 0; i < (std::size_t)err.size(); ++i) { |
| 761 |
|
✗ |
(*e)[(ULong)i] = err[i]; |
| 762 |
|
|
} |
| 763 |
|
✗ |
error = e; |
| 764 |
|
✗ |
return res; |
| 765 |
|
✗ |
} catch (const std::exception& exc) { |
| 766 |
|
✗ |
throw Error(exc.what()); |
| 767 |
|
|
} |
| 768 |
|
|
} |
| 769 |
|
|
|
| 770 |
|
✗ |
CORBA::Boolean Graph::getConfigErrorForEdgeLeaf( |
| 771 |
|
|
ID edgeId, const hpp::floatSeq& leafDofArray, const hpp::floatSeq& dofArray, |
| 772 |
|
|
hpp::floatSeq_out error) { |
| 773 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
| 774 |
|
|
try { |
| 775 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 776 |
|
|
// If steering method is not completely set in the graph, create |
| 777 |
|
|
// one. |
| 778 |
|
✗ |
if (!edge->parentGraph()->problem()->manipulationSteeringMethod() || |
| 779 |
|
✗ |
!edge->parentGraph() |
| 780 |
|
✗ |
->problem() |
| 781 |
|
✗ |
->manipulationSteeringMethod() |
| 782 |
|
✗ |
->innerSteeringMethod()) { |
| 783 |
|
✗ |
problemSolver()->initSteeringMethod(); |
| 784 |
|
✗ |
if (!edge->parentGraph()->problem()->manipulationSteeringMethod() || |
| 785 |
|
✗ |
!edge->parentGraph() |
| 786 |
|
✗ |
->problem() |
| 787 |
|
✗ |
->manipulationSteeringMethod() |
| 788 |
|
✗ |
->innerSteeringMethod()) |
| 789 |
|
✗ |
throw Error("Could not initialize the steering method."); |
| 790 |
|
|
} |
| 791 |
|
✗ |
vector_t err; |
| 792 |
|
✗ |
Configuration_t leafConfig(floatSeqToConfig(robot, leafDofArray, true)); |
| 793 |
|
✗ |
Configuration_t config(floatSeqToConfig(robot, dofArray, true)); |
| 794 |
|
|
bool res = |
| 795 |
|
✗ |
graph()->getConfigErrorForEdgeLeaf(leafConfig, config, edge, err); |
| 796 |
|
✗ |
floatSeq* e = new floatSeq(); |
| 797 |
|
✗ |
e->length((ULong)err.size()); |
| 798 |
|
✗ |
for (std::size_t i = 0; i < (std::size_t)err.size(); ++i) { |
| 799 |
|
✗ |
(*e)[(ULong)i] = err[i]; |
| 800 |
|
|
} |
| 801 |
|
✗ |
error = e; |
| 802 |
|
✗ |
return res; |
| 803 |
|
✗ |
} catch (const std::exception& exc) { |
| 804 |
|
✗ |
throw Error(exc.what()); |
| 805 |
|
|
} |
| 806 |
|
|
} |
| 807 |
|
|
|
| 808 |
|
✗ |
CORBA::Boolean Graph::getConfigErrorForEdgeTarget( |
| 809 |
|
|
ID edgeId, const hpp::floatSeq& leafDofArray, const hpp::floatSeq& dofArray, |
| 810 |
|
|
hpp::floatSeq_out error) { |
| 811 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
| 812 |
|
|
try { |
| 813 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 814 |
|
|
// If steering method is not completely set in the graph, create |
| 815 |
|
|
// one. |
| 816 |
|
✗ |
if (!edge->parentGraph()->problem()->manipulationSteeringMethod() || |
| 817 |
|
✗ |
!edge->parentGraph() |
| 818 |
|
✗ |
->problem() |
| 819 |
|
✗ |
->manipulationSteeringMethod() |
| 820 |
|
✗ |
->innerSteeringMethod()) { |
| 821 |
|
✗ |
problemSolver()->initSteeringMethod(); |
| 822 |
|
✗ |
if (!edge->parentGraph()->problem()->manipulationSteeringMethod() || |
| 823 |
|
✗ |
!edge->parentGraph() |
| 824 |
|
✗ |
->problem() |
| 825 |
|
✗ |
->manipulationSteeringMethod() |
| 826 |
|
✗ |
->innerSteeringMethod()) |
| 827 |
|
✗ |
throw Error("Could not initialize the steering method."); |
| 828 |
|
|
} |
| 829 |
|
✗ |
vector_t err; |
| 830 |
|
✗ |
Configuration_t leafConfig(floatSeqToConfig(robot, leafDofArray, true)); |
| 831 |
|
✗ |
Configuration_t config(floatSeqToConfig(robot, dofArray, true)); |
| 832 |
|
|
bool res = |
| 833 |
|
✗ |
graph()->getConfigErrorForEdgeTarget(leafConfig, config, edge, err); |
| 834 |
|
✗ |
floatSeq* e = new floatSeq(); |
| 835 |
|
✗ |
e->length((ULong)err.size()); |
| 836 |
|
✗ |
for (std::size_t i = 0; i < (std::size_t)err.size(); ++i) { |
| 837 |
|
✗ |
(*e)[(ULong)i] = err[i]; |
| 838 |
|
|
} |
| 839 |
|
✗ |
error = e; |
| 840 |
|
✗ |
return res; |
| 841 |
|
✗ |
} catch (const std::exception& exc) { |
| 842 |
|
✗ |
throw Error(exc.what()); |
| 843 |
|
|
} |
| 844 |
|
|
} |
| 845 |
|
|
|
| 846 |
|
✗ |
void Graph::displayNodeConstraints(hpp::ID nodeId, |
| 847 |
|
|
CORBA::String_out constraints) { |
| 848 |
|
|
try { |
| 849 |
|
✗ |
graph::StatePtr_t state = getComp<graph::State>(nodeId); |
| 850 |
|
✗ |
ConstraintSetPtr_t cs(graph()->configConstraint(state)); |
| 851 |
|
✗ |
std::ostringstream oss; |
| 852 |
|
✗ |
oss << (*cs); |
| 853 |
|
✗ |
constraints = oss.str().c_str(); |
| 854 |
|
✗ |
} catch (const std::exception& exc) { |
| 855 |
|
✗ |
throw Error(exc.what()); |
| 856 |
|
|
} |
| 857 |
|
|
} |
| 858 |
|
|
|
| 859 |
|
✗ |
void Graph::displayEdgeTargetConstraints(hpp::ID edgeId, |
| 860 |
|
|
CORBA::String_out constraints) { |
| 861 |
|
|
try { |
| 862 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 863 |
|
✗ |
ConstraintSetPtr_t cs(graph()->targetConstraint(edge)); |
| 864 |
|
✗ |
std::ostringstream oss; |
| 865 |
|
✗ |
oss << (*cs); |
| 866 |
|
✗ |
constraints = oss.str().c_str(); |
| 867 |
|
✗ |
} catch (const std::exception& exc) { |
| 868 |
|
✗ |
throw Error(exc.what()); |
| 869 |
|
|
} |
| 870 |
|
|
} |
| 871 |
|
|
|
| 872 |
|
✗ |
void Graph::displayEdgeConstraints(hpp::ID edgeId, |
| 873 |
|
|
CORBA::String_out constraints) { |
| 874 |
|
|
try { |
| 875 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 876 |
|
✗ |
ConstraintSetPtr_t cs(graph()->pathConstraint(edge)); |
| 877 |
|
✗ |
std::ostringstream oss; |
| 878 |
|
✗ |
oss << (*cs); |
| 879 |
|
✗ |
constraints = oss.str().c_str(); |
| 880 |
|
✗ |
} catch (const std::exception& exc) { |
| 881 |
|
✗ |
throw Error(exc.what()); |
| 882 |
|
|
} |
| 883 |
|
|
} |
| 884 |
|
|
|
| 885 |
|
✗ |
void Graph::getNodesConnectedByEdge(hpp::ID edgeId, CORBA::String_out from, |
| 886 |
|
|
CORBA::String_out to) { |
| 887 |
|
|
try { |
| 888 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 889 |
|
✗ |
from = edge->stateFrom()->name().c_str(); |
| 890 |
|
✗ |
to = edge->stateTo()->name().c_str(); |
| 891 |
|
✗ |
} catch (const std::exception& exc) { |
| 892 |
|
✗ |
throw Error(exc.what()); |
| 893 |
|
|
} |
| 894 |
|
|
} |
| 895 |
|
|
|
| 896 |
|
✗ |
void Graph::display(const char* filename) { |
| 897 |
|
|
try { |
| 898 |
|
✗ |
std::cout << *graph(); |
| 899 |
|
✗ |
std::ofstream dotfile; |
| 900 |
|
✗ |
dotfile.open(filename); |
| 901 |
|
✗ |
graph()->dotPrint(dotfile); |
| 902 |
|
✗ |
dotfile.close(); |
| 903 |
|
✗ |
} catch (const std::exception& exc) { |
| 904 |
|
✗ |
throw Error(exc.what()); |
| 905 |
|
|
} |
| 906 |
|
|
} |
| 907 |
|
|
|
| 908 |
|
✗ |
void Graph::getHistogramValue(ID edgeId, hpp::floatSeq_out freq, |
| 909 |
|
|
hpp::floatSeqSeq_out values) { |
| 910 |
|
✗ |
graph::LevelSetEdgePtr_t edge = getComp<graph::LevelSetEdge>(edgeId); |
| 911 |
|
|
try { |
| 912 |
|
✗ |
graph::LeafHistogramPtr_t hist = edge->histogram(); |
| 913 |
|
✗ |
floatSeq* _freq = new floatSeq(); |
| 914 |
|
✗ |
floatSeqSeq* _values = new floatSeqSeq(); |
| 915 |
|
✗ |
_freq->length(hist->numberOfBins()); |
| 916 |
|
✗ |
_values->length(hist->numberOfBins()); |
| 917 |
|
✗ |
ULong i = 0; |
| 918 |
|
✗ |
for (graph::LeafHistogram::const_iterator it = hist->begin(); |
| 919 |
|
✗ |
it != hist->end(); ++it) { |
| 920 |
|
✗ |
(*_freq)[i] = (CORBA::Double)it->freq(); |
| 921 |
|
✗ |
floatSeq v; |
| 922 |
|
✗ |
const vector_t& offset = it->value(); |
| 923 |
|
✗ |
v.length((ULong)offset.size()); |
| 924 |
|
✗ |
for (ULong j = 0; j < offset.size(); ++j) v[j] = (CORBA::Double)offset[j]; |
| 925 |
|
✗ |
(*_values)[i] = v; |
| 926 |
|
✗ |
i++; |
| 927 |
|
|
} |
| 928 |
|
✗ |
freq = _freq; |
| 929 |
|
✗ |
values = _values; |
| 930 |
|
✗ |
} catch (const std::exception& exc) { |
| 931 |
|
✗ |
throw Error(exc.what()); |
| 932 |
|
|
} |
| 933 |
|
|
} |
| 934 |
|
|
|
| 935 |
|
✗ |
void Graph::setShort(ID edgeId, CORBA::Boolean isShort) { |
| 936 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 937 |
|
|
try { |
| 938 |
|
✗ |
edge->setShort(isShort); |
| 939 |
|
|
} catch (const std::exception& exc) { |
| 940 |
|
|
throw Error(exc.what()); |
| 941 |
|
|
} |
| 942 |
|
|
} |
| 943 |
|
|
|
| 944 |
|
✗ |
bool Graph::isShort(ID edgeId) { |
| 945 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 946 |
|
|
try { |
| 947 |
|
✗ |
return edge->isShort(); |
| 948 |
|
|
} catch (const std::exception& exc) { |
| 949 |
|
|
throw Error(exc.what()); |
| 950 |
|
|
} |
| 951 |
|
|
} |
| 952 |
|
|
|
| 953 |
|
✗ |
Long Graph::autoBuild(const char* graphName, const Names_t& grippers, |
| 954 |
|
|
const Names_t& objects, const Namess_t& handlesPerObject, |
| 955 |
|
|
const Namess_t& shapesPreObject, const Names_t& envNames, |
| 956 |
|
|
const Rules& rulesList) { |
| 957 |
|
✗ |
std::vector<graph::helper::Rule> rules(rulesList.length()); |
| 958 |
|
|
|
| 959 |
|
✗ |
for (ULong i = 0; i < rulesList.length(); ++i) { |
| 960 |
|
✗ |
setRule(rulesList[i], rules[i]); |
| 961 |
|
|
} |
| 962 |
|
|
try { |
| 963 |
|
|
graph::GraphPtr_t g = graph::helper::graphBuilder( |
| 964 |
|
✗ |
problemSolver(), graphName, |
| 965 |
|
✗ |
corbaServer::toStrings<std::vector<std::string> >(grippers), |
| 966 |
|
✗ |
toObjectVector(objects, handlesPerObject, shapesPreObject), |
| 967 |
|
✗ |
corbaServer::toStrings<std::vector<std::string> >(envNames), rules); |
| 968 |
|
|
|
| 969 |
|
✗ |
return (Long)g->id(); |
| 970 |
|
✗ |
} catch (const std::exception& exc) { |
| 971 |
|
✗ |
throw Error(exc.what()); |
| 972 |
|
|
} |
| 973 |
|
|
} |
| 974 |
|
|
|
| 975 |
|
✗ |
void Graph::setWeight(ID edgeId, const Long weight) { |
| 976 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 977 |
|
|
try { |
| 978 |
|
✗ |
edge->stateFrom()->updateWeight(edge, weight); |
| 979 |
|
✗ |
} catch (const std::exception& exc) { |
| 980 |
|
✗ |
throw Error(exc.what()); |
| 981 |
|
|
} |
| 982 |
|
|
} |
| 983 |
|
|
|
| 984 |
|
✗ |
Long Graph::getWeight(ID edgeId) { |
| 985 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId); |
| 986 |
|
|
try { |
| 987 |
|
✗ |
return (Long)edge->stateFrom()->getWeight(edge); |
| 988 |
|
✗ |
} catch (const std::exception& exc) { |
| 989 |
|
✗ |
throw Error(exc.what()); |
| 990 |
|
|
} |
| 991 |
|
|
} |
| 992 |
|
|
|
| 993 |
|
✗ |
char* Graph::getName(ID elmtId) { |
| 994 |
|
|
try { |
| 995 |
|
✗ |
return corbaServer::c_str(graph()->get(elmtId).lock()->name()); |
| 996 |
|
✗ |
} catch (std::exception& e) { |
| 997 |
|
✗ |
throw Error(e.what()); |
| 998 |
|
|
} |
| 999 |
|
|
} |
| 1000 |
|
|
|
| 1001 |
|
✗ |
void Graph::initialize() { |
| 1002 |
|
|
try { |
| 1003 |
|
✗ |
problemSolver()->initConstraintGraph(); |
| 1004 |
|
✗ |
} catch (const std::exception& exc) { |
| 1005 |
|
✗ |
throw Error(exc.what()); |
| 1006 |
|
|
} |
| 1007 |
|
|
} |
| 1008 |
|
|
|
| 1009 |
|
✗ |
void Graph::getRelativeMotionMatrix(ID edgeId, intSeqSeq_out matrix) { |
| 1010 |
|
|
try { |
| 1011 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId, true); |
| 1012 |
|
✗ |
matrix = matrixToIntSeqSeq(edge->relativeMotion().cast<CORBA::Long>()); |
| 1013 |
|
✗ |
} catch (const std::exception& exc) { |
| 1014 |
|
✗ |
throw Error(exc.what()); |
| 1015 |
|
|
} |
| 1016 |
|
|
} |
| 1017 |
|
|
|
| 1018 |
|
✗ |
void Graph::getSecurityMarginMatrixForEdge(ID edgeId, floatSeqSeq_out matrix) { |
| 1019 |
|
|
try { |
| 1020 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId, true); |
| 1021 |
|
✗ |
matrix = corbaServer::matrixToFloatSeqSeq(edge->securityMargins()); |
| 1022 |
|
✗ |
} catch (const std::exception& exc) { |
| 1023 |
|
✗ |
throw Error(exc.what()); |
| 1024 |
|
|
} |
| 1025 |
|
|
} |
| 1026 |
|
|
|
| 1027 |
|
✗ |
void Graph::setSecurityMarginForEdge(ID edgeId, const char* joint1, |
| 1028 |
|
|
const char* joint2, double margin) { |
| 1029 |
|
|
try { |
| 1030 |
|
✗ |
graph::EdgePtr_t edge = getComp<graph::Edge>(edgeId, true); |
| 1031 |
|
✗ |
DevicePtr_t robot(edge->parentGraph()->robot()); |
| 1032 |
|
✗ |
JointPtr_t j1(robot->getJointByName(joint1)); |
| 1033 |
|
✗ |
JointPtr_t j2(robot->getJointByName(joint2)); |
| 1034 |
|
✗ |
size_type i1 = 0; |
| 1035 |
|
✗ |
if (j1) { |
| 1036 |
|
✗ |
i1 = j1->index(); |
| 1037 |
|
|
} |
| 1038 |
|
✗ |
size_type i2 = 0; |
| 1039 |
|
✗ |
if (j2) { |
| 1040 |
|
✗ |
i2 = j2->index(); |
| 1041 |
|
|
} |
| 1042 |
|
✗ |
edge->securityMarginForPair(i1, i2, margin); |
| 1043 |
|
✗ |
} catch (const std::exception& exc) { |
| 1044 |
|
✗ |
throw Error(exc.what()); |
| 1045 |
|
|
} |
| 1046 |
|
|
} |
| 1047 |
|
|
|
| 1048 |
|
|
} // namespace impl |
| 1049 |
|
|
} // namespace manipulation |
| 1050 |
|
|
} // namespace hpp |
| 1051 |
|
|
|