Commit 309ad40a authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

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

parent 8d8d4855
......@@ -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;
......
......@@ -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)
{
......
......@@ -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)
{
......
......@@ -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.
......
......@@ -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
......
......@@ -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
......
......@@ -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;
......
......@@ -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;
......
......@@ -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,
......
......@@ -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_;
......
......@@ -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
......
......@@ -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_;
......
......@@ -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 ++) {
......
......@@ -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 ++) {
......
......@@ -20,7 +20,7 @@
namespace hpp {
namespace core {
bool Constraint::apply (Configuration_t& configuration)
bool Constraint::apply (ConfigurationOut_t configuration)
{
return impl_compute (configuration);
}
......
......@@ -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)
......
......@@ -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),
......
......@@ -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 ();
......
......@@ -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;
}
......
......@@ -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) {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment