Extend use of Eigen::Ref to input/output vectors and configurations.

include/hpp/core/config-projector.hh

@@ -62,8 +62,8 @@ namespace hpp {
       /// \textbf{q}_{res} = \textbf{q}_{from} + \left(I_n -
       /// J^{+}J(\textbf{q}_{from})\right) (\textbf{q}_{to} - \textbf{q}_{from})
       /// \f]
-      void projectOnKernel (const Configuration_t& from,
-			    const Configuration_t& to, Configuration_t& result);
+      void projectOnKernel (ConfigurationIn_t from,
+			    ConfigurationIn_t to, ConfigurationOut_t result);
 
     protected:
       /// Constructor
@@ -81,15 +81,15 @@ namespace hpp {
 	weak_ = self;
       }
       /// Numerically solve constraint
-      virtual bool impl_compute (Configuration_t& configuration);
+      virtual bool impl_compute (ConfigurationOut_t configuration);
       /// Set locked degrees of freedom to their locked values
-      void computeLockedDofs (Configuration_t& configuration);
+      void computeLockedDofs (ConfigurationOut_t configuration);
 
     private:
       virtual std::ostream& print (std::ostream& os) const;
       virtual void addToConstraintSet (const ConstraintSetPtr_t& constraintSet);
-      void smallToNormal (const vector_t& small, vector_t& normal);
-      void normalToSmall (const vector_t& normal, vector_t& small);
+      void smallToNormal (vectorIn_t small, vectorOut_t normal);
+      void normalToSmall (vectorIn_t normal, vectorOut_t small);
       struct FunctionValueAndJacobian_t {
 	FunctionValueAndJacobian_t (DifferentiableFunctionPtr_t f,
 				    vector_t v, matrix_t j): function (f),
@@ -102,7 +102,7 @@ namespace hpp {
       }; // struct FunctionValueAndJacobian_t
       typedef std::vector < FunctionValueAndJacobian_t > NumericalConstraints_t;
       void resize ();
-      void computeValueAndJacobian (const Configuration_t& configuration);
+      void computeValueAndJacobian (ConfigurationIn_t configuration);
       virtual void addLockedDof (const LockedDofPtr_t& lockedDof);
       void computeIntervals ();
       typedef std::list <LockedDofPtr_t> LockedDofs_t;

include/hpp/core/constraint-set.hh

@@ -67,7 +67,7 @@ namespace hpp {
 	Constraint::init (self);
 	weak_ = self;
       }
-      virtual bool impl_compute (Configuration_t& configuration);
+      virtual bool impl_compute (ConfigurationOut_t configuration);
     private:
       virtual void addToConstraintSet (const ConstraintSetPtr_t& constraintSet)
       {

include/hpp/core/constraint.hh

@@ -37,7 +37,7 @@ namespace hpp {
       /// Function that applies the constraint
       /// \param configuration initial configuration and result
       /// \return true if constraint applied successfully, false if failure.
-      bool apply (Configuration_t& configuration);
+      bool apply (ConfigurationOut_t configuration);
       /// Get name of constraint
       const std::string& name () const
       {
@@ -45,7 +45,7 @@ namespace hpp {
       }
     protected:
       /// User defined implementation of the constraint.
-      virtual bool impl_compute (Configuration_t& configuration) = 0;
+      virtual bool impl_compute (ConfigurationOut_t configuration) = 0;
       /// Constructor
       Constraint (const std::string& name) : name_ (name)
 	{

include/hpp/core/differentiable-function.hh

@@ -33,8 +33,8 @@ namespace hpp {
       /// Evaluate the function at a given parameter.
       ///
       /// \note parameters should be of the correct size.
-      void operator () (vector_t& result,
-			const Configuration_t& argument) const
+      void operator () (vectorOut_t result,
+			ConfigurationIn_t argument) const
       {
 	assert (result.size () == outputSize ());
 	assert (argument.size () == inputSize ());
@@ -44,7 +44,7 @@ namespace hpp {
       ///
       /// \retval jacobian jacobian will be stored in this argument
       /// \param argument point at which the jacobian will be computed
-      void jacobian (matrix_t& jacobian, const Configuration_t& argument) const
+      void jacobian (matrix_t& jacobian, ConfigurationIn_t argument) const
       {
 	assert (argument.size () == inputSize ());
 	assert (jacobian.rows () == outputSize ());
@@ -113,11 +113,11 @@ namespace hpp {
       }
 
       /// User implementation of function evaluation
-      virtual void impl_compute (vector_t& result,
-				 const Configuration_t& argument) const = 0;
+      virtual void impl_compute (vectorOut_t result,
+				 ConfigurationIn_t argument) const = 0;
 
       virtual void impl_jacobian (matrix_t& jacobian,
-				  const Configuration_t& arg) const = 0;
+				  ConfigurationIn_t arg) const = 0;
 
     private:
       /// Dimension of input vector.

include/hpp/core/distance.hh

@@ -28,8 +28,8 @@ namespace hpp {
     /// Abstract class for distance between configurations
     class HPP_CORE_DLLAPI Distance {
     public:
-      virtual value_type operator () (const Configuration_t& q1,
-				  const Configuration_t& q2)
+      virtual value_type operator () (ConfigurationIn_t q1,
+				  ConfigurationIn_t q2)
       {
 	return impl_distance (q1, q2);
       }
@@ -42,8 +42,8 @@ namespace hpp {
 	{
 	}
       /// Derived class should implement this function
-      virtual value_type impl_distance (const Configuration_t& q1,
-				    const Configuration_t& q2) = 0;
+      virtual value_type impl_distance (ConfigurationIn_t q1,
+				    ConfigurationIn_t q2) = 0;
     }; // class Distance
   } //   namespace core
 } // namespace hpp

include/hpp/core/fwd.hh

@@ -69,6 +69,8 @@ namespace hpp {
     BasicConfigurationShooterPtr_t;
     typedef model::CollisionObjectPtr_t CollisionObjectPtr_t;
     typedef model::Configuration_t Configuration_t;
+    typedef model::ConfigurationIn_t ConfigurationIn_t; 
+    typedef model::ConfigurationOut_t ConfigurationOut_t; 
     typedef boost::shared_ptr<model::Configuration_t> ConfigurationPtr_t;
     typedef std::vector <ConfigurationPtr_t> Configurations_t;
     typedef Configurations_t::iterator ConfigIterator_t;
@@ -120,6 +122,8 @@ namespace hpp {
     typedef std::vector <PathPtr_t> Paths_t;
     typedef std::vector <PathVectorPtr_t> PathVectors_t;
     typedef model::vector_t vector_t;
+    typedef model::vectorIn_t vectorIn_t;
+    typedef model::vectorOut_t vectorOut_t;
     typedef boost::shared_ptr <WeighedDistance> WeighedDistancePtr_t;
   } // namespace core
 } // namespace hpp

include/hpp/core/locked-dof.hh

@@ -62,7 +62,7 @@ namespace hpp {
 	Constraint::init (self);
 	weak_ = self;
       }
-      bool impl_compute (Configuration_t& configuration)
+      bool impl_compute (ConfigurationOut_t configuration)
       {
 	configuration [index_] = value_;
 	return true;

include/hpp/core/problem.hh

@@ -160,7 +160,7 @@ namespace hpp {
     private :
       /// Validate configuration and track validation reports.
       bool validateConfig (const DevicePtr_t& device,
-			   const Configuration_t& inConfig) const;
+			   ConfigurationIn_t inConfig) const;
 
       /// Validate initial configuration
       void validateInitConfig () const;

include/hpp/core/steering-method-straight.hh

@@ -36,8 +36,8 @@ namespace hpp {
       {
       }
       /// create a path between two configurations
-      virtual PathPtr_t impl_compute (const Configuration_t& q1,
-				      const Configuration_t& q2) const
+      virtual PathPtr_t impl_compute (ConfigurationIn_t q1,
+				      ConfigurationIn_t q2) const
       {
 	value_type length = (*distance_) (q1, q2);
 	return StraightPath::create (device_.lock (), q1, q2, length,

include/hpp/core/steering-method.hh

@@ -39,8 +39,8 @@ namespace hpp {
     {
     public:
       /// create a path between two configurations
-      PathPtr_t operator() (const Configuration_t& q1,
-			    const Configuration_t& q2) const
+      PathPtr_t operator() (ConfigurationIn_t q1,
+			    ConfigurationIn_t q2) const
       {
 	return impl_compute (q1, q2);
       }
@@ -62,8 +62,8 @@ namespace hpp {
 
     protected:
       /// create a path between two configurations
-      virtual PathPtr_t impl_compute (const Configuration_t& q1,
-				      const Configuration_t& q2) const = 0;
+      virtual PathPtr_t impl_compute (ConfigurationIn_t q1,
+				      ConfigurationIn_t q2) const = 0;
     private:
       /// Set of constraints to apply on the paths produced
       ConstraintSetPtr_t constraints_;

include/hpp/core/straight-path.hh

@@ -52,8 +52,8 @@ namespace hpp {
       /// \param init, end Start and end configurations of the path
       /// \param length Distance between the configurations.
       static StraightPathPtr_t create (const DevicePtr_t& device,
-				       const Configuration_t& init,
-				       const Configuration_t& end,
+				       ConfigurationIn_t init,
+				       ConfigurationIn_t end,
 				       value_type length,
 				       const ConstraintSetPtr_t& constraints =
 				       ConstraintSetPtr_t ())
@@ -72,8 +72,8 @@ namespace hpp {
 
     protected:
       /// Constructor
-      StraightPath (const DevicePtr_t& robot, const Configuration_t& init,
-		    const Configuration_t& end, value_type length,
+      StraightPath (const DevicePtr_t& robot, ConfigurationIn_t init,
+		    ConfigurationIn_t end, value_type length,
 		    const ConstraintSetPtr_t& constraints);
 
       /// Copy constructor

include/hpp/core/weighed-distance.hh

@@ -44,8 +44,8 @@ namespace hpp {
       WeighedDistance (const WeighedDistance& distance);
       void init (WeighedDistanceWkPtr self);
       /// Derived class should implement this function
-      virtual value_type impl_distance (const Configuration_t& q1,
-				    const Configuration_t& q2);
+      virtual value_type impl_distance (ConfigurationIn_t q1,
+				    ConfigurationIn_t q2);
     private:
       DevicePtr_t robot_;
       std::vector <value_type> weights_;

src/config-projector.cc

@@ -118,7 +118,7 @@ namespace hpp {
     }
 
     void ConfigProjector::computeValueAndJacobian
-    (const Configuration_t& configuration)
+    (ConfigurationIn_t configuration)
     {
       size_type row = 0, nbRows = 0;
       for (NumericalConstraints_t::iterator itConstraint =
@@ -147,8 +147,8 @@ namespace hpp {
 
     /// Convert vector of non locked degrees of freedom to vector of
     /// all degrees of freedom
-    void ConfigProjector::smallToNormal (const vector_t& small,
-					 vector_t& normal)
+    void ConfigProjector::smallToNormal (vectorIn_t small,
+					 vectorOut_t normal)
     {
       assert (small.size () + lockedDofs_.size () == robot_->numberDof ());
       assert (normal.size () - robot_->numberDof () == 0);
@@ -162,8 +162,8 @@ namespace hpp {
       }
     }
 
-    void ConfigProjector::normalToSmall (const vector_t& normal,
-					 vector_t& small)
+    void ConfigProjector::normalToSmall (vectorIn_t normal,
+					 vectorOut_t small)
     {
       assert (small.size () + lockedDofs_.size () == robot_->numberDof ());
       assert (abs (normal.size ()) - robot_->numberDof () == 0);
@@ -177,7 +177,7 @@ namespace hpp {
       }
     }
 
-    bool ConfigProjector::impl_compute (Configuration_t& configuration)
+    bool ConfigProjector::impl_compute (ConfigurationOut_t configuration)
     {
       hppDout (info, "before projection: " << configuration.transpose ());
       computeLockedDofs (configuration);
@@ -201,7 +201,8 @@ namespace hpp {
 					 Eigen::ComputeThinV);
 	dqSmall_ = svd.solve(value_);
 	smallToNormal (dqSmall_, dq_);
-	model::integrate (robot_, configuration, -alpha * dq_, configuration);
+	vector_t v (-alpha * dq_);
+	model::integrate (robot_, configuration, v, configuration);
 	// Increase alpha towards alphaMax
 	alpha = alphaMax - .8*(alphaMax - alpha);
 	squareNorm = value_.squaredNorm ();
@@ -221,9 +222,9 @@ namespace hpp {
       return true;
     }
 
-    void ConfigProjector::projectOnKernel (const Configuration_t& from,
-					   const Configuration_t& to,
-					   Configuration_t& result)
+    void ConfigProjector::projectOnKernel (ConfigurationIn_t from,
+					   ConfigurationIn_t to,
+					   ConfigurationOut_t result)
     {
       computeValueAndJacobian (from);
       model::difference (robot_, to, from, toMinusFrom_);
@@ -240,7 +241,7 @@ namespace hpp {
       model::integrate (robot_, from, projMinusFrom_, result);
     }
 
-    void ConfigProjector::computeLockedDofs (Configuration_t& configuration)
+    void ConfigProjector::computeLockedDofs (ConfigurationOut_t configuration)
     {
       for (LockedDofs_t::iterator itLock = lockedDofs_.begin ();
 	   itLock != lockedDofs_.end (); itLock ++) {

src/constraint-set.cc

@@ -37,14 +37,14 @@ namespace hpp {
 	: ConfigProjector (robot, "trivial ConfigProjector", 0, 0)
       {
       }
-      bool impl_compute (Configuration_t& configuration)
+      bool impl_compute (ConfigurationOut_t configuration)
       {
 	computeLockedDofs (configuration);
 	return true;
       }
     }; // class ConfigProjectorTrivial
 
-    bool ConstraintSet::impl_compute (Configuration_t& configuration)
+    bool ConstraintSet::impl_compute (ConfigurationOut_t configuration)
     {
       for (Constraints_t::iterator itConstraint = constraints_.begin ();
 	   itConstraint != constraints_.end (); itConstraint ++) {

src/constraint.cc

@@ -20,7 +20,7 @@
 
 namespace hpp {
   namespace core {
-    bool Constraint::apply (Configuration_t& configuration)
+    bool Constraint::apply (ConfigurationOut_t configuration)
     {
       return impl_compute (configuration);
     }

src/diffusing-planner.cc

@@ -31,7 +31,7 @@
 
 namespace hpp {
   namespace core {
-    extern std::string displayConfig (const Configuration_t& q);
+    extern std::string displayConfig (ConfigurationIn_t q);
 
     DiffusingPlannerPtr_t DiffusingPlanner::createWithRoadmap
     (const Problem& problem, const RoadmapPtr_t& roadmap)

src/node.cc

@@ -23,7 +23,7 @@
 
 namespace hpp {
   namespace core {
-    extern std::string displayConfig (const Configuration_t& q);
+    extern std::string displayConfig (ConfigurationIn_t q);
     Node::Node (const ConfigurationPtr_t& configuration,
 		ConnectedComponentPtr_t connectedComponent) :
       configuration_ (configuration),

src/problem.cc

@@ -28,7 +28,7 @@
 namespace hpp {
   namespace core {
 
-    extern std::string displayConfig (const Configuration_t& q);
+    extern std::string displayConfig (ConfigurationIn_t q);
 
     // ======================================================================
 
@@ -154,7 +154,7 @@ namespace hpp {
     }
 
     bool Problem::validateConfig (const DevicePtr_t& device,
-				  const Configuration_t& config) const
+				  ConfigurationIn_t config) const
     {
       device->currentConfiguration (config);
       device->computeForwardKinematics ();

src/random-shortcut.cc

@@ -33,14 +33,16 @@ namespace hpp {
     // Compute the length of a vector of paths assuming that each element
     // is optimal for the given distance.
     static value_type pathLength (const PathVectorPtr_t& path,
-			      const DistancePtr_t& distance)
+				  const DistancePtr_t& distance)
     {
       value_type result = 0;
       for (std::size_t i=0; i<path->numberPaths (); ++i) {
 	const PathPtr_t& element (path->pathAtRank (i));
 	const value_type& tmin (element->timeRange ().first);
 	const value_type& tmax (element->timeRange ().second);
-	result += (*distance) ((*element) (tmin), (*element) (tmax));
+	Configuration_t q1 ((*element) (tmax));
+	Configuration_t q2 ((*element) (tmax));
+	result += (*distance) (q1, q2);
       }
       return result;
     }

src/roadmap.cc

@@ -28,7 +28,7 @@
 namespace hpp {
   namespace core {
 
-    std::string displayConfig (const Configuration_t& q)
+    std::string displayConfig (ConfigurationIn_t q)
     {
       std::ostringstream oss;
       for (size_type i=0; i < q.size (); ++i) {