diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 6f257c787215fd10342e26c29dce8b0e32c45c75..516b7b78b45ecf322a1fb5dc8ecc0ae9a35cc3d1 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -48,7 +48,6 @@ typedef std::vector<pinocchio::FrameIndex> FrameIndices_t;
typedef pinocchio::Configuration_t Configuration_t;
typedef pinocchio::ConfigurationIn_t ConfigurationIn_t;
typedef pinocchio::ConfigurationOut_t ConfigurationOut_t;
-typedef core::ConfigurationPtr_t ConfigurationPtr_t;
typedef pinocchio::GripperPtr_t GripperPtr_t;
typedef pinocchio::LiegroupElement LiegroupElement;
typedef pinocchio::LiegroupSpace LiegroupSpace;
diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh
index ef5937f5c9b97795f8672e387691afc4a310528a..a6adec40ac27f6fbb1ed33f86f44dc95f41cc49f 100644
--- a/include/hpp/manipulation/manipulation-planner.hh
+++ b/include/hpp/manipulation/manipulation-planner.hh
@@ -65,7 +65,7 @@ class HPP_MANIPULATION_DLLAPI ManipulationPlanner
/// \retval validPath the longest valid path (possibly of length 0),
/// resulting from the extension.
/// \return True if the returned path is valid.
- bool extend(RoadmapNodePtr_t q_near, const ConfigurationPtr_t& q_rand,
+ bool extend(RoadmapNodePtr_t q_near, ConfigurationIn_t q_rand,
core::PathPtr_t& validPath);
/// Get the number of occurrence of each errors.
diff --git a/include/hpp/manipulation/path-planner/states-path-finder.hh b/include/hpp/manipulation/path-planner/states-path-finder.hh
index 44c442c51459363d930feac4a78dd4622e57c8ac..30e4dcd2883b1555b0da6dd7c932b1a7bd4f881b 100644
--- a/include/hpp/manipulation/path-planner/states-path-finder.hh
+++ b/include/hpp/manipulation/path-planner/states-path-finder.hh
@@ -117,7 +117,7 @@ class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner {
/// \return a Configurations_t from q1 to q2 if found. An empty
/// vector if a path could not be built.
core::Configurations_t computeConfigList(ConfigurationIn_t q1,
- ConfigurationPtr_t q2);
+ ConfigurationIn_t q2);
// access functions for Python interface
std::vector<std::string> constraintNamesFromSolverAtWaypoint(std::size_t wp);
@@ -246,7 +246,7 @@ class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner {
NumericalConstraints_t goalConstraints_;
bool goalDefinedByConstraints_;
// Variables used across several calls to oneStep
- ConfigurationPtr_t q1_, q2_;
+ Configuration_t q1_, q2_;
core::Configurations_t configList_;
std::size_t idxConfigList_;
size_type nTryConfigList_;
diff --git a/include/hpp/manipulation/path-planner/transition-planner.hh b/include/hpp/manipulation/path-planner/transition-planner.hh
index 6e41cc97fb3aacf22a5cc4ab77bb2dd1d6132feb..0e6240f3f30f5d09147bbe7d2d02610e348b3b0f 100644
--- a/include/hpp/manipulation/path-planner/transition-planner.hh
+++ b/include/hpp/manipulation/path-planner/transition-planner.hh
@@ -108,7 +108,7 @@ class HPP_MANIPULATION_DLLAPI TransitionPlanner : public core::PathPlanner {
/// \retval status a message in case of failure.
/// If a path projector has been selected, the path is tested for
/// continuity.
- PathPtr_t directPath(const Configuration_t& q1, const Configuration_t& q2,
+ PathPtr_t directPath(ConfigurationIn_t q1, ConfigurationIn_t q2,
bool validate, bool& success, std::string& status);
/// Optimize path using the selected path optimizers
/// \param path input path
diff --git a/include/hpp/manipulation/roadmap-node.hh b/include/hpp/manipulation/roadmap-node.hh
index f281285f4e9c223c425857dd32d610f2a578c9c8..ad821f319565a665c3260059ace432f200d4490e 100644
--- a/include/hpp/manipulation/roadmap-node.hh
+++ b/include/hpp/manipulation/roadmap-node.hh
@@ -41,10 +41,10 @@ namespace hpp {
namespace manipulation {
class HPP_MANIPULATION_DLLAPI RoadmapNode : public core::Node {
public:
- RoadmapNode(const ConfigurationPtr_t& configuration)
+ RoadmapNode(ConfigurationIn_t configuration)
: core::Node(configuration), state_() {}
- RoadmapNode(const ConfigurationPtr_t& configuration,
+ RoadmapNode(ConfigurationIn_t configuration,
ConnectedComponentPtr_t cc);
/// \name Cache
diff --git a/include/hpp/manipulation/roadmap.hh b/include/hpp/manipulation/roadmap.hh
index de34c4c14aa670389c74d997b6731d7e08eeb3be..831385672505b94cc85c9aa5b0d9c564c4a601ce 100644
--- a/include/hpp/manipulation/roadmap.hh
+++ b/include/hpp/manipulation/roadmap.hh
@@ -68,7 +68,7 @@ class HPP_MANIPULATION_DLLAPI Roadmap : public core::Roadmap {
/// Get the nearest neighbor in a given graph::Node and in a given
/// ConnectedComponent.
RoadmapNodePtr_t nearestNodeInState(
- const ConfigurationPtr_t& configuration,
+ ConfigurationIn_t configuration,
const ConnectedComponentPtr_t& connectedComponent,
const graph::StatePtr_t& state, value_type& minDistance) const;
@@ -104,7 +104,7 @@ class HPP_MANIPULATION_DLLAPI Roadmap : public core::Roadmap {
Roadmap(const core::DistancePtr_t& distance, const core::DevicePtr_t& robot);
/// Node factory
- core::NodePtr_t createNode(const ConfigurationPtr_t& config) const;
+ core::NodePtr_t createNode(ConfigurationIn_t config) const;
void init(const RoadmapPtr_t& shPtr) {
Parent::init(shPtr);
diff --git a/src/astar.hh b/src/astar.hh
index ae9508cc784af683e4785d543737952b42d49f63..1b3ccd74ee00d56c5c7c437f9e1167e940b2555f 100644
--- a/src/astar.hh
+++ b/src/astar.hh
@@ -137,8 +137,8 @@ class Astar {
inline value_type heuristic(RoadmapNodePtr_t node,
RoadmapNodePtr_t to) const {
- const ConfigurationPtr_t& config = node->configuration();
- return (*distance_)(*config, *to->configuration());
+ const Configuration_t& config = node->configuration();
+ return (*distance_)(config, to->configuration());
}
inline value_type edgeCost(const core::EdgePtr_t& edge) const {
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index c4c5409bdfffafb6b9a775c1024e1b5d0a7317e8..b5a727e62649c98f114795a66a8d28bd43a488a0 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -359,7 +359,7 @@ bool Edge::build(core::PathPtr_t& path, ConfigurationIn_t q1,
bool Edge::generateTargetConfig(core::NodePtr_t nStart,
ConfigurationOut_t q) const {
- return generateTargetConfig(*(nStart->configuration()), q);
+ return generateTargetConfig(nStart->configuration(), q);
}
bool Edge::generateTargetConfig(ConfigurationIn_t qStart,
@@ -577,7 +577,7 @@ bool LevelSetEdge::generateTargetConfig(ConfigurationIn_t qStart,
hppDout(warning, "Edge " << name() << ": Distrib is empty");
return false;
}
- const Configuration_t& qLeaf = *(distrib()->configuration());
+ const Configuration_t& qLeaf = distrib()->configuration();
return generateTargetConfigOnLeaf(qStart, qLeaf, q);
}
@@ -592,8 +592,8 @@ bool LevelSetEdge::generateTargetConfig(core::NodePtr_t nStart,
hppDout(warning, "Edge " << name() << ": Distrib is empty");
return false;
}
- const Configuration_t &qLeaf = *(distrib()->configuration()),
- qStart = *(nStart->configuration());
+ const Configuration_t &qLeaf = distrib()->configuration(),
+ qStart = nStart->configuration();
return generateTargetConfigOnLeaf(qStart, qLeaf, q);
}
diff --git a/src/graph/state-selector.cc b/src/graph/state-selector.cc
index e68e669fc66630a8ad6a549795d57e0551d87a9d..e0fbe7fdf997823ccd6263847b19c60dc327a702 100644
--- a/src/graph/state-selector.cc
+++ b/src/graph/state-selector.cc
@@ -91,7 +91,7 @@ StatePtr_t StateSelector::getState(ConfigurationIn_t config) const {
StatePtr_t StateSelector::getState(RoadmapNodePtr_t node) const {
if (!node->cacheUpToDate())
- node->graphState(getState(*(node->configuration())));
+ node->graphState(getState(node->configuration()));
return node->graphState();
}
diff --git a/src/graph/statistics.cc b/src/graph/statistics.cc
index f651e154c5561adfae069ecb1a57818b887710a0..40c686d7ee5cfd5b2437ed55065db2b8ad3ef7f5 100644
--- a/src/graph/statistics.cc
+++ b/src/graph/statistics.cc
@@ -153,8 +153,8 @@ LeafHistogram::LeafHistogram(const Foliation f) : f_(f), threshold_(0) {
}
void LeafHistogram::add(const RoadmapNodePtr_t& n) {
- if (!f_.contains(*n->configuration())) return;
- iterator it = insert(LeafBin(f_.parameter(*n->configuration()), &threshold_));
+ if (!f_.contains(n->configuration())) return;
+ iterator it = insert(LeafBin(f_.parameter(n->configuration()), &threshold_));
it->push_back(n);
if (numberOfObservations() % 10 == 0) {
hppDout(info, *this);
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index d91a1d2e69901a6a0c9c8f30a4f17a049368dd5a..f8b953ed4ba8cb6aa79f4db27126a3ba15305c63 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -75,7 +75,7 @@ graph::StatePtr_t getState(const graph::GraphPtr_t graph,
if (mnode != NULL)
return mnode->graphState();
else
- return graph->getState(*node->configuration());
+ return graph->getState(node->configuration());
}
core::PathPtr_t connect(const Configuration_t& q1, const Configuration_t& q2,
@@ -177,13 +177,13 @@ void ManipulationPlanner::oneStep() {
core::Nodes_t newNodes;
core::PathPtr_t path;
- typedef std::tuple<core::NodePtr_t, ConfigurationPtr_t, core::PathPtr_t>
+ typedef std::tuple<core::NodePtr_t, Configuration_t, core::PathPtr_t>
DelayedEdge_t;
typedef std::vector<DelayedEdge_t> DelayedEdges_t;
DelayedEdges_t delayedEdges;
// Pick a random node
- ConfigurationPtr_t q_rand = shooter_->shoot();
+ Configuration_t q_rand = shooter_->shoot();
// Extend each connected component
for (core::ConnectedComponents_t::const_iterator itcc =
@@ -210,12 +210,11 @@ void ManipulationPlanner::oneStep() {
value_type t_final = path->timeRange().second;
if (t_final != path->timeRange().first) {
bool success;
- ConfigurationPtr_t q_new(
- new Configuration_t(path->eval(t_final, success)));
+ Configuration_t q_new(path->eval(t_final, success));
assert(success);
assert(!path->constraints() ||
- path->constraints()->isSatisfied(*q_new));
- assert(problem_->constraintGraph()->getState(*q_new));
+ path->constraints()->isSatisfied(q_new));
+ assert(problem_->constraintGraph()->getState(q_new));
delayedEdges.push_back(DelayedEdge_t(near, q_new, path));
}
}
@@ -226,7 +225,7 @@ void ManipulationPlanner::oneStep() {
// Insert delayed edges
for (const auto& edge : delayedEdges) {
const core::NodePtr_t& near = std::get<0>(edge);
- const ConfigurationPtr_t& q_new = std::get<1>(edge);
+ Configuration_t q_new = std::get<1>(edge);
const core::PathPtr_t& validPath = std::get<2>(edge);
core::NodePtr_t newNode = roadmap()->addNode(q_new);
roadmap()->addEdge(near, newNode, validPath);
@@ -263,21 +262,21 @@ void ManipulationPlanner::oneStep() {
}
bool ManipulationPlanner::extend(RoadmapNodePtr_t n_near,
- const ConfigurationPtr_t& q_rand,
+ ConfigurationIn_t q_rand,
core::PathPtr_t& validPath) {
graph::GraphPtr_t graph = problem_->constraintGraph();
PathProjectorPtr_t pathProjector = problem_->pathProjector();
pinocchio::DevicePtr_t robot(problem_->robot());
value_type eps(graph->errorThreshold());
// Select next node in the constraint graph.
- const ConfigurationPtr_t q_near = n_near->configuration();
+ const Configuration_t q_near = n_near->configuration();
HPP_START_TIMECOUNTER(chooseEdge);
graph::EdgePtr_t edge = graph->chooseEdge(n_near);
HPP_STOP_TIMECOUNTER(chooseEdge);
if (!edge) {
return false;
}
- qProj_ = *q_rand;
+ qProj_ = q_rand;
HPP_START_TIMECOUNTER(generateTargetConfig);
SuccessStatistics& es = edgeStat(edge);
if (!edge->generateTargetConfig(n_near, qProj_)) {
@@ -286,7 +285,7 @@ bool ManipulationPlanner::extend(RoadmapNodePtr_t n_near,
es.addFailure(reasons_[PROJECTION]);
return false;
}
- if (pinocchio::isApprox(robot, qProj_, *q_near, eps)) {
+ if (pinocchio::isApprox(robot, qProj_, q_near, eps)) {
es.addFailure(reasons_[FAILURE]);
es.addFailure(reasons_[PATH_PROJECTION_ZERO]);
return false;
@@ -294,7 +293,7 @@ bool ManipulationPlanner::extend(RoadmapNodePtr_t n_near,
HPP_STOP_TIMECOUNTER(generateTargetConfig);
core::PathPtr_t path;
HPP_START_TIMECOUNTER(buildPath);
- if (!edge->build(path, *q_near, qProj_)) {
+ if (!edge->build(path, q_near, qProj_)) {
HPP_STOP_TIMECOUNTER(buildPath);
es.addFailure(reasons_[FAILURE]);
es.addFailure(reasons_[STEERING_METHOD]);
@@ -396,7 +395,7 @@ inline std::size_t ManipulationPlanner::tryConnectToRoadmap(
value_type distance;
for (core::Nodes_t::const_iterator itn1 = nodes.begin(); itn1 != nodes.end();
++itn1) {
- const Configuration_t& q1(*(*itn1)->configuration());
+ const Configuration_t& q1((*itn1)->configuration());
graph::StatePtr_t s1 = getState(graph, *itn1);
connectSucceed = false;
for (core::ConnectedComponents_t::const_iterator itcc =
@@ -411,7 +410,7 @@ inline std::size_t ManipulationPlanner::tryConnectToRoadmap(
bool _2to1 = (*itn1)->isInNeighbor(*itn2);
assert(!_1to2 || !_2to1);
- const Configuration_t& q2(*(*itn2)->configuration());
+ const Configuration_t& q2((*itn2)->configuration());
graph::StatePtr_t s2 = getState(graph, *itn2);
assert(q1 != q2);
@@ -445,7 +444,7 @@ inline std::size_t ManipulationPlanner::tryConnectNewNodes(
std::size_t nbConnection = 0;
for (core::Nodes_t::const_iterator itn1 = nodes.begin(); itn1 != nodes.end();
++itn1) {
- const Configuration_t& q1(*(*itn1)->configuration());
+ const Configuration_t& q1((*itn1)->configuration());
graph::StatePtr_t s1 = getState(graph, *itn1);
for (core::Nodes_t::const_iterator itn2 = std::next(itn1);
@@ -455,7 +454,7 @@ inline std::size_t ManipulationPlanner::tryConnectNewNodes(
bool _1to2 = (*itn1)->isOutNeighbor(*itn2);
bool _2to1 = (*itn1)->isInNeighbor(*itn2);
assert(!_1to2 || !_2to1);
- const Configuration_t& q2(*(*itn2)->configuration());
+ const Configuration_t& q2((*itn2)->configuration());
graph::StatePtr_t s2 = getState(graph, *itn2);
assert(q1 != q2);
diff --git a/src/path-planner/end-effector-trajectory.cc b/src/path-planner/end-effector-trajectory.cc
index 4a708bb2b930c394be6c2f38e7c4b54b3491dc3b..360fbb4070c2d6942a866ee4173213e054e342b6 100644
--- a/src/path-planner/end-effector-trajectory.cc
+++ b/src/path-planner/end-effector-trajectory.cc
@@ -73,7 +73,7 @@ void EndEffectorTrajectory::startSolve() {
throw std::runtime_error(msg);
}
- if (!problem()->initConfig()) {
+ if (problem()->initConfig().size() == 0) {
std::string msg("No init config in problem.");
hppDout(error, msg);
throw std::runtime_error(msg);
@@ -154,7 +154,7 @@ void EndEffectorTrajectory::oneStep() {
// Generate a vector if configuration where the first one is the initial
// configuration and the following ones are random configurations
std::vector<core::Configuration_t> qs(
- configurations(*problem()->initConfig()));
+ configurations(problem()->initConfig()));
if (qs.empty()) {
hppDout(info, "Failed to generate initial configs.");
return;
@@ -238,10 +238,9 @@ void EndEffectorTrajectory::oneStep() {
// - insert q_init as initial configuration of the roadmap,
// - insert final configuration as goal node in the roadmap,
// - add a roadmap edge between them and stop.
- roadmap()->initNode(make_shared<Configuration_t>(steps.col(0)));
+ roadmap()->initNode(steps.col(0));
core::NodePtr_t init = roadmap()->initNode();
- core::NodePtr_t goal = roadmap()->addGoalNode(
- make_shared<Configuration_t>(steps.col(nDiscreteSteps_)));
+ core::NodePtr_t goal = roadmap()->addGoalNode(steps.col(nDiscreteSteps_));
roadmap()->addEdge(init, goal, path);
success = true;
if (feasibilityOnly_) break;
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index 4f79a8ea31f67aefeac4b7fbe5c2440675140153..20eb2515570917a056eb803191c859a85761e486 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -77,7 +77,7 @@ static void displayRoadmap(const core::RoadmapPtr_t& roadmap) {
for (auto cc : roadmap->connectedComponents()) {
hppDout(info, " CC " << i++);
for (auto n : cc->nodes()) {
- hppDout(info, pinocchio::displayConfig(*(n->configuration())));
+ hppDout(info, pinocchio::displayConfig(n->configuration()));
}
}
}
@@ -96,8 +96,8 @@ StatesPathFinder::StatesPathFinder(const core::ProblemConstPtr_t& problem,
lastBuiltTransitions_(),
goalConstraints_(),
goalDefinedByConstraints_(false),
- q1_(0x0),
- q2_(0x0),
+ q1_(),
+ q2_(),
configList_(),
idxConfigList_(0),
nTryConfigList_(0),
@@ -409,7 +409,7 @@ struct StatesPathFinder::OptimizationData {
// Number of trials to generate each waypoint configuration
// _solveGoalConfig: whether we need to solve for goal configuration
OptimizationData(const core::ProblemConstPtr_t _problem,
- const Configuration_t& _q1, const ConfigurationPtr_t& _q2,
+ ConfigurationIn_t _q1, ConfigurationIn_t _q2,
const Edges_t& transitions, const bool _solveGoalConfig)
: N(_solveGoalConfig ? transitions.size() : transitions.size() - 1),
nq(_problem->robot()->configSize()),
@@ -421,8 +421,8 @@ struct StatesPathFinder::OptimizationData {
M_rhs(),
M_status() {
if (!_solveGoalConfig) {
- assert(_q2);
- q2 = *_q2;
+ assert(_q2.size() > 0);
+ q2 = _q2;
}
waypoint.setZero();
for (auto solver : solvers) {
@@ -893,7 +893,7 @@ bool StatesPathFinder::analyseOptimizationProblem(
size_type tries = 0;
Configuration_t q;
do {
- q = *(problem()->configurationShooter()->shoot());
+ q = problem()->configurationShooter()->shoot();
preInitializeRHS(j, q);
status = solver.solve(q, constraints::solver::lineSearch::Backtracking());
} while ((status != Solver_t::SUCCESS) && (++tries <= nTriesMax));
@@ -1044,8 +1044,8 @@ bool StatesPathFinder::analyseOptimizationProblem2(
}
}
// initialize the right hand side with the initial config
- analyseSolver.rightHandSideFromConfig(*q1_);
- if (analyseSolver.isSatisfied(*q1_)) {
+ analyseSolver.rightHandSideFromConfig(q1_);
+ if (analyseSolver.isSatisfied(q1_)) {
return true;
}
hppDout(info,
@@ -1091,8 +1091,7 @@ void StatesPathFinder::initializeRHS(std::size_t j) {
void StatesPathFinder::initWPRandom(std::size_t wp) {
assert(wp >= 1 && wp <= (std::size_t)optData_->waypoint.cols());
initializeRHS(wp - 1);
- optData_->waypoint.col(wp - 1) =
- *(problem()->configurationShooter()->shoot());
+ optData_->waypoint.col(wp - 1) = problem()->configurationShooter()->shoot();
}
void StatesPathFinder::initWPNear(std::size_t wp) {
assert(wp >= 1 && wp <= (std::size_t)optData_->waypoint.cols());
@@ -1278,15 +1277,12 @@ bool StatesPathFinder::solveOptimizationProblem() {
core::Configurations_t StatesPathFinder::getConfigList() const {
OptimizationData& d = *optData_;
core::Configurations_t pv;
- ConfigurationPtr_t q1(new Configuration_t(d.q1));
- pv.push_back(q1);
+ pv.push_back(d.q1);
for (std::size_t i = 0; i < d.N; ++i) {
- ConfigurationPtr_t q(new Configuration_t(d.waypoint.col(i)));
- pv.push_back(q);
+ pv.push_back(d.waypoint.col(i));
}
if (!goalDefinedByConstraints_) {
- ConfigurationPtr_t q2(new Configuration_t(d.q2));
- pv.push_back(q2);
+ pv.push_back(d.q2);
}
return pv;
}
@@ -1297,7 +1293,7 @@ core::Configurations_t StatesPathFinder::getConfigList() const {
// [one of potential goal states if goal defined as set of constraints]
// and compute waypoint configurations in each state.
core::Configurations_t StatesPathFinder::computeConfigList(
- ConfigurationIn_t q1, ConfigurationPtr_t q2) {
+ ConfigurationIn_t q1, ConfigurationIn_t q2) {
const graph::GraphPtr_t& graph(problem_->constraintGraph());
GraphSearchData& d = *graphData_;
@@ -1352,8 +1348,7 @@ core::Configurations_t StatesPathFinder::computeConfigList(
}
}
core::Configurations_t empty_path;
- ConfigurationPtr_t q(new Configuration_t(q1));
- empty_path.push_back(q);
+ empty_path.push_back(q1);
return empty_path;
}
@@ -1374,7 +1369,7 @@ void StatesPathFinder::startSolve() {
PathPlanner::startSolve();
assert(problem_);
q1_ = problem_->initConfig();
- assert(q1_);
+ assert(q1_.size() > 0);
// core::PathProjectorPtr_t pathProjector
// (core::pathProjector::Progressive::create(inStateProblem_, 0.01));
@@ -1383,7 +1378,7 @@ void StatesPathFinder::startSolve() {
const graph::GraphPtr_t& graph(problem_->constraintGraph());
graphData_.reset(new GraphSearchData());
GraphSearchData& d = *graphData_;
- d.s1 = graph->getState(*q1_);
+ d.s1 = graph->getState(q1_);
d.maxDepth = problem_->getParameter("StatesPathFinder/maxDepth").intValue();
d.queue1.push_back(d.addInitState());
@@ -1416,7 +1411,7 @@ void StatesPathFinder::startSolve() {
throw std::logic_error(os.str().c_str());
}
q2_ = q2s[0];
- d.s2.push_back(graph->getState(*q2_));
+ d.s2.push_back(graph->getState(q2_));
} else {
TaskTargetPtr_t taskTarget(HPP_DYNAMIC_PTR_CAST(TaskTarget, target));
if (!taskTarget) {
@@ -1425,7 +1420,7 @@ void StatesPathFinder::startSolve() {
"either a configuration or a set of constraints.";
throw std::logic_error(os.str().c_str());
}
- assert(!q2_);
+ assert(q2_.size() == 0);
goalDefinedByConstraints_ = true;
goalConstraints_ = taskTarget->constraints();
hppDout(info, "goal defined as a set of constraints");
@@ -1460,15 +1455,15 @@ void StatesPathFinder::startSolve() {
void StatesPathFinder::oneStep() {
if (idxConfigList_ == 0) {
// TODO: accommodate when goal is a set of constraints
- assert(q1_);
- configList_ = computeConfigList(*q1_, q2_);
+ assert(q1_.size()>0);
+ configList_ = computeConfigList(q1_, q2_);
if (configList_.size() <= 1) { // max depth reached
reset();
throw core::path_planning_failed("Maximal depth reached.");
}
}
size_t& idxSol = graphData_->idxSol;
- ConfigurationPtr_t q1, q2;
+ Configuration_t q1, q2;
if (idxConfigList_ >= configList_.size() - 1) {
reset();
throw core::path_planning_failed(
@@ -1481,8 +1476,8 @@ void StatesPathFinder::oneStep() {
core::ConstraintSetPtr_t constraints(HPP_DYNAMIC_PTR_CAST(
core::ConstraintSet, edge->pathConstraint()->copy()));
// Initialize right hand side
- constraints->configProjector()->rightHandSideFromConfig(*q1);
- assert(constraints->isSatisfied(*q2));
+ constraints->configProjector()->rightHandSideFromConfig(q1);
+ assert(constraints->isSatisfied(q2));
inStateProblem_->constraints(constraints);
inStateProblem_->pathValidation(edge->pathValidation());
inStateProblem_->steeringMethod(edge->steeringMethod());
diff --git a/src/path-planner/transition-planner.cc b/src/path-planner/transition-planner.cc
index 53f415d490e9bc4c5f8e0ae1689b01eae7c83eec..bbde195044749ba32541cdf4aadbe6c1e04ae701 100644
--- a/src/path-planner/transition-planner.cc
+++ b/src/path-planner/transition-planner.cc
@@ -68,7 +68,7 @@ void TransitionPlanner::startSolve() {
" no constraints. You probably forgot to select "
"the transition.");
innerProblem_->constraints()->configProjector()->rightHandSideFromConfig(
- *(innerProblem_->initConfig()));
+ innerProblem_->initConfig());
// Forward maximal number of iterations to inner planner
innerPlanner_->maxIterations(this->maxIterations());
// Forward timeout to inner planner
@@ -88,12 +88,12 @@ core::PathVectorPtr_t TransitionPlanner::planPath(const Configuration_t qInit,
if (configProjector) {
configProjector->rightHandSideFromConfig(qInit);
}
- ConfigurationPtr_t q(new Configuration_t(qInit));
+ Configuration_t q(qInit);
innerProblem_->initConfig(q);
innerProblem_->resetGoalConfigs();
for (size_type r = 0; r < qGoals.rows(); ++r) {
- ConfigurationPtr_t q(new Configuration_t(qGoals.row(r)));
- if (!configProjector->isSatisfied(*q)) {
+ Configuration_t q(qGoals.row(r));
+ if (!configProjector->isSatisfied(q)) {
std::ostringstream os;
os << "hpp::manipulation::TransitionPlanner::computePath: "
<< "goal configuration at rank " << r
@@ -110,8 +110,8 @@ core::PathVectorPtr_t TransitionPlanner::planPath(const Configuration_t qInit,
return path;
}
-core::PathPtr_t TransitionPlanner::directPath(const Configuration_t& q1,
- const Configuration_t& q2,
+core::PathPtr_t TransitionPlanner::directPath(ConfigurationIn_t q1,
+ ConfigurationIn_t q2,
bool validate, bool& success,
std::string& status) {
core::PathPtr_t res(innerProblem_->steeringMethod()->steer(q1, q2));
diff --git a/src/problem-target/astar.hh b/src/problem-target/astar.hh
index f3062b8b13b13ecc173c1eb689956e333e5348f5..5fc3d2cbddcda15aab077169da08bae7e3c9f795 100644
--- a/src/problem-target/astar.hh
+++ b/src/problem-target/astar.hh
@@ -144,13 +144,13 @@ class HPP_MANIPULATION_LOCAL Astar {
}
value_type heuristic(const RoadmapNodePtr_t node) const {
- const ConfigurationPtr_t config = node->configuration();
+ const Configuration_t& config = node->configuration();
value_type res = std::numeric_limits<value_type>::infinity();
for (core::NodeVector_t::const_iterator itGoal =
roadmap_->goalNodes().begin();
itGoal != roadmap_->goalNodes().end(); ++itGoal) {
- ConfigurationPtr_t goal = (*itGoal)->configuration();
- value_type dist = (*distance_)(*config, *goal);
+ const Configuration_t& goal = (*itGoal)->configuration();
+ value_type dist = (*distance_)(config, goal);
if (dist < res) {
res = dist;
}
diff --git a/src/roadmap-node.cc b/src/roadmap-node.cc
index 0d0f81c53bd949c269c9528cf2ede50e03eba6a8..02700589d5afe1d029b9a677df2b6361f7592c72 100644
--- a/src/roadmap-node.cc
+++ b/src/roadmap-node.cc
@@ -32,7 +32,7 @@
namespace hpp {
namespace manipulation {
-RoadmapNode::RoadmapNode(const ConfigurationPtr_t& configuration,
+RoadmapNode::RoadmapNode(ConfigurationIn_t configuration,
ConnectedComponentPtr_t cc)
: core::Node(configuration, cc), state_() {}
} // namespace manipulation
diff --git a/src/roadmap.cc b/src/roadmap.cc
index 26e4fd50c5fe00d963e1b48e9cdd8f882154cbfa..1b174b8ab2c9abc7b90ed583ec224ac9789823aa 100644
--- a/src/roadmap.cc
+++ b/src/roadmap.cc
@@ -91,7 +91,7 @@ void Roadmap::constraintGraph(const graph::GraphPtr_t& graph) {
}
RoadmapNodePtr_t Roadmap::nearestNodeInState(
- const ConfigurationPtr_t& configuration,
+ ConfigurationIn_t configuration,
const ConnectedComponentPtr_t& connectedComponent,
const graph::StatePtr_t& state, value_type& minDistance) const {
core::NodePtr_t result = NULL;
@@ -103,7 +103,7 @@ RoadmapNodePtr_t Roadmap::nearestNodeInState(
// << std::endl;
for (RoadmapNodes_t::const_iterator itNode = roadmapNodes.begin();
itNode != roadmapNodes.end(); ++itNode) {
- value_type d = (*distance())(*(*itNode)->configuration(), *configuration);
+ value_type d = (*distance())((*itNode)->configuration(), configuration);
if (d < minDistance) {
minDistance = d;
result = *itNode;
@@ -112,7 +112,7 @@ RoadmapNodePtr_t Roadmap::nearestNodeInState(
return static_cast<RoadmapNode*>(result);
}
-core::NodePtr_t Roadmap::createNode(const ConfigurationPtr_t& q) const {
+core::NodePtr_t Roadmap::createNode(ConfigurationIn_t q) const {
// call RoadmapNode constructor with new manipulation connected component
RoadmapNodePtr_t node = new RoadmapNode(q, ConnectedComponent::create(weak_));
LeafConnectedCompPtr_t sc = WeighedLeafConnectedComp::create(weak_.lock());
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index a9ac705889ba4bad620b581f8550cf93fa88186d..6da90a05f805024957151f8b0c2602924c9e239a 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -534,7 +534,7 @@ bool CrossStateOptimization::solveOptimizationProblem(
.intValue());
while (status != Solver_t::SUCCESS && nbTry < nRandomConfigs) {
- d.waypoint.col(j) = *(problem()->configurationShooter()->shoot());
+ d.waypoint.col(j) = problem()->configurationShooter()->shoot();
status = solver.solve(d.waypoint.col(j),
constraints::solver::lineSearch::Backtracking());
++nbTry;
diff --git a/src/weighed-distance.cc b/src/weighed-distance.cc
index f3a0e980b2edc6af15f11a01e532aa18cc2eb8f1..388f729397590d320be56837e2b9488cc1b35100 100644
--- a/src/weighed-distance.cc
+++ b/src/weighed-distance.cc
@@ -86,7 +86,7 @@ value_type WeighedDistance::impl_distance(ConfigurationIn_t q1,
value_type WeighedDistance::impl_distance(core::NodePtr_t n1,
core::NodePtr_t n2) const {
- Configuration_t &q1 = *n1->configuration(), q2 = *n2->configuration();
+ const Configuration_t &q1 = n1->configuration(), q2 = n2->configuration();
value_type d = core::WeighedDistance::impl_distance(q1, q2);
graph::Edges_t pes =