diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7da4fbca6d59a3d29706f6a5bd2635d90a5b0960..b3c09fd0ce62a7d0f7e752c48eb5c9e39b96c5b0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -30,6 +30,8 @@ SET(PROJECT_DESCRIPTION "Classes for manipulation planning.")
SETUP_HPP_PROJECT()
+LIST(APPEND PKG_CONFIG_ADDITIONAL_VARIABLES cmake_plugin)
+
# Activate test using UR5 if requested
SET (TEST_UR5 FALSE CACHE BOOL "Activate tests using ur5")
@@ -103,6 +105,7 @@ SET (${PROJECT_NAME}_HEADERS
)
ADD_SUBDIRECTORY(src)
+ADD_SUBDIRECTORY(plugins)
ADD_SUBDIRECTORY(tests)
# Add dependency toward hpp-manipulation library in pkg-config file.
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index f203ccd72db9a2d27d962bd5f747e0b6a6bf32e5..e101c1c1f4515361323ef00a357e15481df0900a 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -32,8 +32,8 @@ namespace hpp {
typedef pinocchio::JointPtr_t JointPtr_t;
typedef pinocchio::JointIndex JointIndex;
typedef std::vector<JointIndex> JointIndices_t;
- typedef se3::FrameIndex FrameIndex;
- typedef std::vector<se3::FrameIndex> FrameIndices_t;
+ typedef pinocchio::FrameIndex FrameIndex;
+ 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;
diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..43cd510111f9edd08f4d425c0d92893ccdaafe47
--- /dev/null
+++ b/plugins/CMakeLists.txt
@@ -0,0 +1,22 @@
+# Copyright (c) 2019, Joseph Mirabel
+# Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+#
+# This file is part of hpp-manipulation.
+# hpp-manipulation is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation, either version
+# 3 of the License, or (at your option) any later version.
+#
+# hpp-manipulation is distributed in the hope that it will be
+# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+# General Lesser Public License for more details. You should have
+# received a copy of the GNU Lesser General Public License along with
+# hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+INCLUDE(${HPP_CORE_CMAKE_PLUGIN})
+
+ADD_PLUGIN(manipulation-spline-gradient-based
+ SOURCES spline-gradient-based.cc
+ LINK_DEPENDENCIES ${PROJECT_NAME} ${PROJECT_NAME}-gpl hpp-core-gpl
+ PKG_CONFIG_DEPENDENCIES hpp-core)
diff --git a/plugins/spline-gradient-based.cc b/plugins/spline-gradient-based.cc
new file mode 100644
index 0000000000000000000000000000000000000000..19634d3a8e8daddd606736d0abe827e6c88c2090
--- /dev/null
+++ b/plugins/spline-gradient-based.cc
@@ -0,0 +1,47 @@
+// Copyright (c) 2019, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#include <hpp/core/plugin.hh>
+#include <hpp/core/problem-solver.hh>
+
+#include <hpp/manipulation/path-optimization/spline-gradient-based.hh>
+
+namespace hpp {
+ namespace manipulation {
+ class SplineGradientBasedPlugin : public core::ProblemSolverPlugin
+ {
+ public:
+ SplineGradientBasedPlugin ()
+ : ProblemSolverPlugin ("SplineGradientBasedPlugin", "0.0")
+ {}
+
+ protected:
+ virtual bool impl_initialize (core::ProblemSolverPtr_t ps)
+ {
+ // ps->pathOptimizers.add ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>::createFromCore);
+ // ps->pathOptimizers.add ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>::createFromCore);
+ // ps->pathOptimizers.add ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>::createFromCore);
+ ps->pathOptimizers.add ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 1>::createFromCore);
+ // ps->pathOptimizers.add ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 2>::createFromCore);
+ ps->pathOptimizers.add ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 3>::createFromCore);
+
+ return true;
+ }
+ };
+ } // namespace manipulation
+} // namespace hpp
+
+HPP_CORE_DEFINE_PLUGIN(hpp::manipulation::SplineGradientBasedPlugin)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index aca6483826da17dbf7743f6cb1cbe5d143730c6a..e4b11af20c19dfb4c61f93c7073815efbce8a9c3 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -47,7 +47,6 @@ SET(SOURCES
graph/dot.cc
path-optimization/random-shortcut.cc
- path-optimization/spline-gradient-based.cc
path-optimization/enforce-transition-semantic.cc
problem-target/state.cc
@@ -71,5 +70,20 @@ PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-constraints)
IF(HPP_WHOLEBODY_STEP_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-wholebody-step)
ENDIF(HPP_WHOLEBODY_STEP_FOUND)
+TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES})
INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)
+
+ADD_LIBRARY(${LIBRARY_NAME}-gpl SHARED
+ path-optimization/spline-gradient-based.cc
+ )
+
+PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME}-gpl hpp-core)
+PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME}-gpl hpp-statistics)
+PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME}-gpl hpp-constraints)
+IF(HPP_WHOLEBODY_STEP_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME}-gpl hpp-wholebody-step)
+ENDIF(HPP_WHOLEBODY_STEP_FOUND)
+TARGET_LINK_LIBRARIES(${LIBRARY_NAME}-gpl ${LIBRARY_NAME} hpp-core-gpl)
+
+INSTALL(TARGETS ${LIBRARY_NAME}-gpl DESTINATION lib)
diff --git a/src/device.cc b/src/device.cc
index d4f01097a99d17ae8b4814dca13fe3f719d6e73d..680a5eacb3e273aefc4ac9fa24608ab09a239276 100644
--- a/src/device.cc
+++ b/src/device.cc
@@ -29,8 +29,7 @@
namespace hpp {
namespace manipulation {
- using se3::Frame;
- using se3::FrameIndex;
+ using ::pinocchio::Frame;
pinocchio::DevicePtr_t Device::clone () const
{
@@ -48,7 +47,7 @@ namespace hpp {
pinocchio::GeomModel& gm = geomModel();
// The root frame should be the first frame.
Frame& rootFrame = m.frames[idxs[0]];
- if (rootFrame.type == se3::JOINT) {
+ if (rootFrame.type == ::pinocchio::JOINT) {
JointIndex jid = m.getJointId (rootFrame.name);
m.jointPlacements[jid] = t;
return;
@@ -61,16 +60,16 @@ namespace hpp {
if (frame.parent == rootFrame.parent) {
// frame is between rootFrame and next moving joints.
frame.placement = shift * frame.placement;
- if (frame.type == se3::BODY) {
+ if (frame.type == ::pinocchio::BODY) {
// Update the geometry object placement.
for (std::size_t k = 0; k < gm.geometryObjects.size(); ++k)
{
- se3::GeometryObject& go = gm.geometryObjects[k];
+ ::pinocchio::GeometryObject& go = gm.geometryObjects[k];
if (go.parentFrame == idxs[i])
go.placement = shift * go.placement;
}
}
- } else if ((frame.type == se3::JOINT) && (rootFrame.parent == m.parents[frame.parent])) {
+ } else if ((frame.type == ::pinocchio::JOINT) && (rootFrame.parent == m.parents[frame.parent])) {
// frame corresponds to a child joint of rootFrame.parent
m.jointPlacements[frame.parent] = shift * m.jointPlacements[frame.parent];
}
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 2531b0fe0a7064018a989344087b345ebdba408d..35291767d7ad2c453e70046b90e5d38f1cf2694b 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -1103,8 +1103,8 @@ namespace hpp {
// Loop over all frames of object, detect joint and create locked
// joint.
assert (robot.frameIndices.has (od.name));
- BOOST_FOREACH (const se3::FrameIndex& f, robot.frameIndices.get (od.name)) {
- if (model.frames[f].type != se3::JOINT) continue;
+ BOOST_FOREACH (const FrameIndex& f, robot.frameIndices.get (od.name)) {
+ if (model.frames[f].type != ::pinocchio::JOINT) continue;
const JointIndex j = model.frames[f].parent;
JointPtr_t oj (Joint::create (ps->robot(), j));
LiegroupSpacePtr_t space (oj->configurationSpace ());
diff --git a/src/handle.cc b/src/handle.cc
index 0027bac7fe197aeebe43893672040e3611c41f27..f08012ea891103a2fe5b18db50ab6be4ccd41a76 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -21,11 +21,12 @@
#include <boost/assign/list_of.hpp>
-#include <pinocchio/multibody/joint/joint.hpp>
+#include <pinocchio/multibody/joint/joint-generic.hpp>
#include <hpp/util/debug.hh>
#include <hpp/pinocchio/joint.hh>
+#include <hpp/pinocchio/joint-collection.hh>
#include <hpp/pinocchio/gripper.hh>
#include <hpp/constraints/differentiable-function.hh>
@@ -52,7 +53,7 @@ namespace hpp {
context ("Grasp complement");
}
- inline void impl_compute (LiegroupElement&, vectorIn_t) const {}
+ inline void impl_compute (pinocchio::LiegroupElementRef, vectorIn_t) const {}
inline void impl_jacobian (matrixOut_t, vectorIn_t) const {}
};
}
@@ -65,7 +66,7 @@ namespace hpp {
const JointPtr_t& joint = handle.joint();
if ( joint
&& !joint->parentJoint()
- && joint->jointModel().shortname() == se3::JointModelFreeFlyer::classname()) {
+ && joint->jointModel().shortname() == ::pinocchio::JointModelFreeFlyer::classname()) {
return true;
}
return false;
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 8f8d0955c67e7e70123936b7359b544023dee2fa..e2ed4d0b03dbb24a1197efb0db4f47b25bdd5477 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -52,7 +52,6 @@
#include "hpp/manipulation/graph-optimizer.hh"
#include "hpp/manipulation/graph-path-validation.hh"
#include "hpp/manipulation/graph-node-optimizer.hh"
-#include "hpp/manipulation/path-optimization/spline-gradient-based.hh"
#include "hpp/manipulation/path-optimization/random-shortcut.hh"
#include "hpp/manipulation/path-optimization/enforce-transition-semantic.hh"
#include "hpp/manipulation/problem-target/state.hh"
@@ -130,13 +129,6 @@ namespace hpp {
pathProjectors.add ("RecursiveHermite",
createPathProjector <core::pathProjector::RecursiveHermite>);
- // pathOptimizers.add ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>::createFromCore);
- // pathOptimizers.add ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>::createFromCore);
- // pathOptimizers.add ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>::createFromCore);
- pathOptimizers.add ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 1>::createFromCore);
- // pathOptimizers.add ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 2>::createFromCore);
- pathOptimizers.add ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 3>::createFromCore);
-
steeringMethods.add ("Graph-SteeringMethodStraight",
steeringMethod::Graph::create <core::SteeringMethodStraight>);
steeringMethods.add ("Graph-Straight",
@@ -174,9 +166,12 @@ namespace hpp {
void ProblemSolver::resetProblem ()
{
- if (problem_)
- delete (problem_);
- initializeProblem (new Problem (robot_));
+ ProblemPtr_t p (new Problem (robot_));
+ if (problem_) {
+ p->parameters = problem_->parameters;
+ delete problem_;
+ }
+ initializeProblem (p);
}
void ProblemSolver::initializeProblem (ProblemPtr_t problem)
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index 737d5e5dc6b2d6d37eadb77756e062a064ae8a79..850392f90152496c79659140ff95237f1ec2d5ca 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -27,6 +27,7 @@
#include <pinocchio/multibody/model.hpp>
#include <hpp/pinocchio/configuration.hh>
+#include <hpp/pinocchio/joint-collection.hh>
#include <hpp/constraints/affine-function.hh>
#include <hpp/constraints/locked-joint.hh>
@@ -550,7 +551,7 @@ namespace hpp {
bool _saturate (vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& sat)
{
bool ret = false;
- const se3::Model& model = robot->model();
+ const pinocchio::Model& model = robot->model();
for (std::size_t i = 1; i < model.joints.size(); ++i) {
const size_type jnq = model.joints[i].nq();
diff --git a/src/steering-method/cross-state-optimization/function.cc b/src/steering-method/cross-state-optimization/function.cc
index b74aa61b27cb4aa08879d58a0744fec05310afec..10443ef1030de854a4f4b9cb0aaeb202857f5fb2 100644
--- a/src/steering-method/cross-state-optimization/function.cc
+++ b/src/steering-method/cross-state-optimization/function.cc
@@ -55,7 +55,7 @@ namespace hpp {
}
protected:
- void impl_compute (LiegroupElement& y, vectorIn_t arg) const
+ void impl_compute (pinocchio::LiegroupElementRef y, vectorIn_t arg) const
{
inner_->value(y, arg.segment (sa_.first, sa_.second));
}
@@ -109,7 +109,7 @@ namespace hpp {
}
protected:
- void impl_compute (LiegroupElement& y, vectorIn_t arg) const
+ void impl_compute (pinocchio::LiegroupElementRef y, vectorIn_t arg) const
{
inner_->value(l_, arg.segment (lsa_.first, lsa_.second));
inner_->value(r_, arg.segment (rsa_.first, rsa_.second));
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 5a419baf6749912bedada57e6df641575561ba02..16fb181e673ef53388d98ffa23070056e2edc08b 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -49,4 +49,3 @@ IF (TEST_UR5)
ADD_TESTCASE (test-constraintgraph FALSE)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DTEST_UR5")
ENDIF ()
-# ADD_TESTCASE (path-projection FALSE) # moved in hpp-core
diff --git a/tests/path-projection.cc b/tests/path-projection.cc
deleted file mode 100644
index fdb17625d810ff5bfe38454f1b56fd183c64b977..0000000000000000000000000000000000000000
--- a/tests/path-projection.cc
+++ /dev/null
@@ -1,218 +0,0 @@
-// Copyright (c) 2014, LAAS-CNRS
-// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
-//
-// This file is part of hpp-manipulation.
-// hpp-manipulation is free software: you can redistribute it
-// and/or modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation, either version
-// 3 of the License, or (at your option) any later version.
-//
-// hpp-manipulation is distributed in the hope that it will be
-// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-// General Lesser Public License for more details. You should have
-// received a copy of the GNU Lesser General Public License along with
-// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
-
-#define ARM_LENGTH 1
-#define FOREARM_LENGTH 1
-#define STEP_PATH (value_type)0.01
-
-#include <math.h>
-
-#include <boost/assign/list_of.hpp>
-
-#include <pinocchio/multibody/model.hpp>
-
-#include <hpp/pinocchio/device.hh>
-#include <hpp/pinocchio/joint.hh>
-#include <hpp/pinocchio/configuration.hh>
-
-#include <hpp/core/path-projector/progressive.hh>
-#include <hpp/core/steering-method-straight.hh>
-#include <hpp/core/numerical-constraint.hh>
-#include <hpp/core/weighed-distance.hh>
-#include <hpp/core/config-projector.hh>
-#include <hpp/core/constraint-set.hh>
-#include <hpp/core/problem.hh>
-
-#include <hpp/constraints/generic-transformation.hh>
-
-#define REQUIRE_MESSAGE(b,m) do {\
- if (!b) {\
- std::cout << m << std::endl;\
- exit(1);\
- }}\
- while (0)
-
-using hpp::pinocchio::Device;
-using hpp::pinocchio::DevicePtr_t;
-using hpp::pinocchio::JointPtr_t;
-using hpp::pinocchio::BodyPtr_t;
-using hpp::pinocchio::Configuration_t;
-using hpp::pinocchio::value_type;
-using hpp::pinocchio::Transform3f;
-
-using hpp::constraints::Position;
-using hpp::constraints::PositionPtr_t;
-using hpp::constraints::matrix3_t;
-using hpp::constraints::vector3_t;
-
-using hpp::core::StraightPath;
-using hpp::core::StraightPathPtr_t;
-using hpp::core::Path;
-using hpp::core::PathPtr_t;
-using hpp::core::WeighedDistance;
-using hpp::core::WeighedDistancePtr_t;
-using hpp::core::SteeringMethodStraight;
-using hpp::core::SteeringMethodPtr_t;
-using hpp::core::ConstraintSet;
-using hpp::core::ConstraintSetPtr_t;
-using hpp::core::ConfigProjector;
-using hpp::core::ConfigProjectorPtr_t;
-using hpp::constraints::Implicit;
-
-using boost::assign::list_of;
-
-using hpp::core::pathProjector::ProgressivePtr_t;
-using hpp::core::pathProjector::Progressive;
-
-using hpp::core::Problem;
-using hpp::core::ProblemPtr_t;
-
-const matrix3_t I3 (matrix3_t::Identity());
-
-namespace hpp_test {
-
- template<typename JointModel>
- static void addJointAndBody(se3::Model & model,
- const se3::JointModelBase<JointModel> & joint,
- const vector3_t shift,
- const se3::JointIndex & parent,
- const std::string & name)
- {
- typedef typename JointModel::ConfigVector_t CV;
- typedef typename JointModel::TangentVector_t TV;
-
- se3::JointIndex idx;
-
- idx = model.addJoint(parent, joint, se3::SE3 (I3, shift), name,
- TV::Random() + TV::Constant(1),
- TV::Random() + TV::Constant(1),
- CV::Random() - CV::Constant(1),
- CV::Random() + CV::Constant(1));
- model.addJointFrame (idx);
-
- model.appendBodyToJoint(idx,se3::Inertia::Random(),se3::SE3::Identity());
- model.addBodyFrame(name + "_BODY", idx);
- }
-
- DevicePtr_t createRobot ()
- {
- DevicePtr_t robot = Device::create ("test");
- se3::Model& model = robot->model();
-
- // lleg
- addJointAndBody(model,se3::JointModelRX(),vector3_t::Zero(), 0, "ARM");
- addJointAndBody(model,se3::JointModelRX(),vector3_t(0, ARM_LENGTH, 0), 1, "FOREARM");
- model.addFrame(se3::Frame("EE", 2, model.getFrameId("FOREARM"), se3::SE3::Identity(), se3::FIXED_JOINT));
-
- robot->createData();
- robot->controlComputation((Device::Computation_t) (Device::JOINT_POSITION | Device::JACOBIAN));
- robot->currentConfiguration(robot->neutralConfiguration());
- robot->computeForwardKinematics();
- return robot;
- }
-
- std::ostream& print (std::ostream& os, const Configuration_t& c)
- {
- os << "[ \t";
- for (int i = 0; i < c.size () - 1; i++)
- os << c[i] << ",\t";
- return os << c[c.size() - 1] << "]";
- }
-
- std::ostream& printpath (std::ostream& os, const Path& p)
- {
- value_type t = p.timeRange ().first;
- bool noWarning;
- while (t < p.timeRange().second) {
- print (os, p (t, noWarning)) << ",";
- t += STEP_PATH;
- }
- return print (os, p (p.timeRange ().second, noWarning));
- }
-
- namespace pythonscript {
- std::ostream& start (std::ostream& os)
- {
- os << "import numpy as np\n"
- << "import matplotlib.pyplot as plt\n";
- return os;
- }
-
- std::ostream& pathToVar (std::ostream& os, const Path& p, const std::string& var)
- {
- os << var << " = np.array ([\n";
- return printpath (os, p) << "\n])\n";
- }
-
- std::ostream& plot (std::ostream& os, const std::string& var)
- {
- os << "fig = plt.figure ()\n"
- << "axes = fig.gca ()\n"
- << "axes.plot (" << var << "[:,0], " << var << "[:,1], '-.', marker='+', markeredgecolor='r', markersize=5)\n"
- << "axes.set_xlabel ('Theta 1')\n"
- << "axes.set_ylabel ('Theta 2')\n"
- << "axes.set_title ('" << var << "')\n"
- << "plt.show ()\n";
- return os;
- }
- }
-}
-
-int main (int , char**) {
- DevicePtr_t r = hpp_test::createRobot ();
- JointPtr_t ee = r->getJointByName ("FOREARM");
- vector3_t target (0, (ARM_LENGTH + FOREARM_LENGTH ) / 2, 0),
- origin (0, FOREARM_LENGTH, 0);
- PositionPtr_t c = Position::create ("Pos", r, ee,
- Transform3f(I3, origin), Transform3f(I3, target),
- list_of (false)(true)(false).convert_to_container<std::vector<bool> >());
- ConstraintSetPtr_t cs = ConstraintSet::create (r, "test-cs");
- ConfigProjectorPtr_t proj = ConfigProjector::create (r, "test", 1e-4, 20);
- proj->add (Implicit::create (c));
- cs->addConstraint (proj);
- ProblemPtr_t problem (new Problem (r));
- WeighedDistancePtr_t dist = WeighedDistance::createWithWeight
- (r, list_of (1)(1));
- problem->distance (dist);
- SteeringMethodPtr_t sm (SteeringMethodStraight::create (problem));
- const WeighedDistance& d = *dist;
- ProgressivePtr_t pp_ptr = Progressive::create (dist, sm, 0.1);
- Progressive pp = *pp_ptr;
-
- Configuration_t qinit (2), qgoal (2);
- qinit[0] = M_PI / 3; qinit[1] = -M_PI / 3;
- qgoal[0] = -M_PI / 3; qgoal[1] = M_PI / 3;
- Configuration_t qinitp = qinit,
- qgoalp = qgoal;
- REQUIRE_MESSAGE (cs->apply (qinitp), "Could not project " << qinit);
- REQUIRE_MESSAGE (cs->apply (qgoalp), "Could not project " << qgoal);
- value_type l = d (qinitp, qgoalp);
- StraightPathPtr_t sp = StraightPath::create (r, qinitp, qgoalp, l);
- std::ostream& out = std::cout;
- hpp_test::pythonscript::start (out);
- std::string n = "direct_path";
- hpp_test::pythonscript::pathToVar (out, *sp, n);
- hpp_test::pythonscript::plot (out, n);
- sp = HPP_STATIC_PTR_CAST (StraightPath, sp->copy (cs));
- n = "naive_projection";
- hpp_test::pythonscript::pathToVar (out, *sp, n);
- hpp_test::pythonscript::plot (out, n);
- PathPtr_t pProj;
- pp.apply (sp, pProj);
- n = "proj_progressive";
- hpp_test::pythonscript::pathToVar (out, *pProj, n);
- hpp_test::pythonscript::plot (out, n);
-}