Commit 5a62897d authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Update to new version of hpp-core

  cut dependency to kitelab and jrl-dynamics,
  implement roadmap, problem, path planner...
parent 6f44438d
......@@ -35,22 +35,45 @@ SET(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib)
SETUP_PROJECT()
# Activate hpp-util logging if requested
SET (HPP_DEBUG FALSE CACHE BOOL "trigger hpp-util debug output")
IF (HPP_DEBUG)
MESSAGE(STATUS "Activate hpp-util logging.")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_DEBUG")
ENDIF()
# Declare Headers
SET(${PROJECT_NAME}_HEADERS
include/hpp/core/configuration-shooter.hh
include/hpp/core/config-projector.hh
include/hpp/core/connected-component.hh
include/hpp/core/constraint.hh
include/hpp/core/constraint-set.hh
include/hpp/core/differentiable-function.hh
include/hpp/core/diffusing-planner.hh
include/hpp/core/discretized-collision-checking.hh
include/hpp/core/distance.hh
include/hpp/core/edge.hh
include/hpp/core/fwd.hh
include/hpp/core/collision-pair.hh
include/hpp/core/planner.hh
include/hpp/core/locked-dof.hh
include/hpp/core/node.hh
include/hpp/core/path.hh
include/hpp/core/path-optimizer.hh
include/hpp/core/path-planner.hh
include/hpp/core/path-validation.hh
include/hpp/core/path-vector.hh
include/hpp/core/plan-and-optimize.hh
include/hpp/core/problem.hh
include/hpp/core/problem-solver.hh
include/hpp/core/random-shortcut.hh
include/hpp/core/roadmap.hh
include/hpp/core/steering-method.hh
include/hpp/core/steering-method-straight.hh
include/hpp/core/straight-path.hh
include/hpp/core/weighed-distance.hh
)
ADD_REQUIRED_DEPENDENCY("hpp-util >= 0.8")
ADD_REQUIRED_DEPENDENCY("hpp-kwsplus >= 2.3")
ADD_REQUIRED_DEPENDENCY("hpp-model >= 2.1")
ADD_REQUIRED_DEPENDENCY("roboptim-trajectory >= 1.0")
# Add dependency toward hpp-model library in pkg-config file.
PKG_CONFIG_APPEND_LIBS("hpp-core")
......
INPUT = @CMAKE_SOURCE_DIR@/include \
@CMAKE_SOURCE_DIR@/doc \
@CMAKE_SOURCE_DIR@/doc/additionalDoc
@CMAKE_SOURCE_DIR@/doc
TAGFILES = @ROBOPTIM_TRAJECTORY_DOXYGENDOCDIR@/roboptim-trajectory.doxytag=@ROBOPTIM_TRAJECTORY_DOXYGENDOCDIR@ \
@HPP_MODEL_DOXYGENDOCDIR@/hpp-model.doxytag=@HPP_MODEL_DOXYGENDOCDIR@
HIDE_FRIEND_COMPOUNDS = YES
/** \mainpage
\section hppCore_sec_intro Introduction
This package implements basic classes used as interfaces with KineoWorks.
The main classes are:
\li hpp::core::Problem: define a canonical path planning problem.
\li hpp::core::Planner: contains a vector of above path planning problems to implement iterative planning algorithms that use several instaciations of a robot.
This package and depending packages implementing algorithms can be
used with or without GUI, depending on whether we are developing and
debugging new algorithms or we want to run the algorithms on-board a robot.
Upon some events (a problem is added in hpp::core::Planner object, a path has been planned, ...), notifications are
sent.
\li When working with the GUI, these notification are caught by the interface and objects are added in the view,
\li when working without interface, the notification have no effet.
*/
/*!
\page porting Porting code from version 2.3 to 2.4
This page explains how to port client code from version 2.3 to 2.4.
\section hpp_core_porting_sec_goal_conf Goal configurations
From version 2.4 on, several goal configurations can be defined by users.
In some motion planning problems, this feature is usefull. For instance,
in manipulation planning problems, the goal is defined as a submanifold
of the configuration space. In this case, it is efficient to sample the
goal sub-manifold and to add goal configurations to the roadmap builder.
The following methods have changed or are new in 2.4
\subsection hpp_core_porting_subsec_planner hpp::core::Planner class
\li hpp::core::Planner::initConfIthProblem(unsigned int rank, const CkwsConfigShPtr& config) asserts that preconditions are satisfied and returns void.
\li hpp::core::Planner::goalConfIthProblem(unsigned int rank) const now return a vector
of shared pointers to goal configurations instead of a single shared
pointer.
\li hpp::core::Planner::goalConfIthProblem(unsigned int rank, const CkwsConfigShPtr config) has been replaced by hpp::core::Planner::addGoalConfIthProblem(unsigned int rank, const CkwsConfigShPtr& config). This new method returns void.
\li hpp::core::Planner::void resetGoalConfIthProblem (unsigned int rank) has been
added.
\subsection hpp_core_porting_subsec_problem hpp::core::Problem class
\li hpp::core::Problem::initConfig (const CkwsConfigShPtr& inConfig) asserts that
preconditions are satisfied and returns void.
\li hpp::core::Problem::goalConfig (const CkwsConfigShPtr& inConfig) has been replaced
by hpp::core::Problem::addGoalConfig (const CkwsConfigShPtr& inConfig). This latter
method returns void.
\li hpp::core::Problem::resetGoalConfig () has been added.
*/
Constraint
addToSet (set):
set.push_back (self)
addLockedDof (lockedDof):
pass
ConfigProjector ConstraintSet
addLockedDof (ld): addToSet (set):
lockedDofs_.push_back (ld) for c in constraints_:
c.addToSet (set)
addToSet (set):
if set.configProjector_: addLockedDof (ld):
throw for c in constraints_:
if set.lockedDof_: c.addLockedDof (ld)
throw
set.configProjector_ = self addConstraint (c):
Constraint::addToSet (set) c.addToSet (self)
LockedDof
addToSet (set):
Constraint::addToSet (self)
set.lockedDof_ = true
set.addLockedDof (self)
\ No newline at end of file
/// \mainpage
/// \section hppCore_sec_intro Introduction
///
/// This package implements path planning algorithms for kinematic chains. Kinematic
/// chains are represented by class hpp::model::Device.
///
/// The main classes are:
/// \li hpp::core::Problem: defines a canonical path planning problem,
/// \li hpp::core::PathPlanner: implements an algorithm to solve a problem,
/// \li hpp::core::Roadmap: stores a network of collision-free paths
/// \li hpp::core::SteeringMethod: builds paths between configurations taking
/// into account kinematic constraints.
/// \li hpp::core::Path: paths for a robot.
\documentclass {article}
\newcommand\reals{\mathbf{R}}
\begin{document}
\paragraph {Projection on the kernel of a matrix $J$}
$$
J\in \reals^{m\times n}
$$
SVD decomposition
\begin{eqnarray*}
J &=& U S V^T \ \ \ U\in O(m)\ \ \ V\in O(n)\\
J^{+} &=& V S^{+} U^T \\
J^{+} J &=& V S^{+} S V^T \\
&=& V \left(\begin{array} {cc}
I_m & O_{m\times n-m} \\
O_{n-m\times m} & O_{n-m\times n-m}
\end{array}\right) V^T
\end{eqnarray*}
where $m$ is the (full) rank of $J$.
\begin{eqnarray*}
I_{n} - J^{+} J &=& V \left(\begin{array} {cc}
O_m & O_{m\times n-m} \\
O_{n-m\times m} & I_{n-m}
\end{array}\right) V^T \\
&=& \left(\begin{array}{cc}
V_{11} & V_{12} \\
V_{21} & V_{22} \end{array}\right)
\left(\begin{array} {cc}
O_m & O_{m\times n-m} \\
O_{n-m\times m} & I_{n-m}
\end{array}\right)
\left(\begin{array}{cc}
V^T_{11} & V^T_{21} \\
V^T_{12} & V^T_{22} \end{array}\right)\\
&=&
\left(\begin{array} {cc}
O_m & V_{12} \\
O_{n-m\times m} & V_{22}
\end{array}\right)
\left(\begin{array}{cc}
V^T_{11} & V^T_{21} \\
V^T_{12} & V^T_{22} \end{array}\right)\\
&=&
\left(\begin{array}{cc}
V_{12}V^T_{12} & V_{12}V^T_{22} \\
V_{22}V^T_{12} & V_{22}V^T_{22} \end{array}\right)
\end{eqnarray*}
\paragraph {Projection on the kernel of a matrix $J$}
$$
J\in \reals^{m\times n}
$$
\begin{eqnarray*}
J^T P &=& QR
\end{eqnarray*}
where $P\in O(m)$ is a permutation matrix, $Q\in O(n)$ and $R\in\reals^{n\times m}$ is an upper triangular matrix.
\begin{eqnarray*}
J &=& PLQ^T
\end{eqnarray*}
where $L = R^T$.
\begin{eqnarray*}
L &=& \left(\begin{array}{cc} L_1 & 0 \end{array}\right)\\
L^+ &=& \left(\begin{array}{c} L_1^-1 \\ 0 \end{array}\right)\\
L^+ L &=& \left(\begin{array}{cc}
I_r & 0 \\
0 & 0 \end{array}\right)
\end{eqnarray*}
where $r$ is the rank of $L$.
\begin{eqnarray*}
J^+ J &=& \left(\begin{array}{cc} Q_1 & Q_0\end{array}\right)
\left(\begin{array}{cc} I_r & 0 \\ 0 & 0\end{array}\right)
\left(\begin{array}{cc} Q_1 & Q_0\end{array}\right)^T \\
&=& Q_1 Q_1^T\\
I_n - J^+ J &=& Q_0 Q_0^T
\end{eqnarray*}
\end{document}
//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core 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-core 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-core If not, see
// <http://www.gnu.org/licenses/>.
#ifndef HPP_CORE_CONFIG_PROJECTOR_HH
# define HPP_CORE_CONFIG_PROJECTOR_HH
# include <roboptim/core/differentiable-function.hh>
# include <hpp/core/config.hh>
# include <hpp/core/constraint.hh>
namespace hpp {
namespace core {
/// Implicit non-linear constraint
///
/// Defined by a list of vector-valued functions and solved numerically
/// by Newton Raphson like method.
/// Store locked degrees of freedom for performance optimisation.
class HPP_CORE_DLLAPI ConfigProjector : public Constraint
{
public:
/// Return shared pointer to new object
/// \param robot robot the constraint applies to.
/// \param errorThreshold norm of the value of the constraint under which
/// the constraint is considered satified,
/// \param maxIterations maximal number of iteration in the resolution of
/// the constraint.
static ConfigProjectorPtr_t create (const DevicePtr_t& robot,
const std::string& name,
value_type errorThreshold,
size_type maxIterations);
/// Add constraint
void addConstraint (const DifferentiableFunctionPtr_t& constraint);
/// Get robot
const DevicePtr_t& robot () const
{
return robot_;
}
/// Project configuration "to" on constraint tangent space in "from"
///
/// \param from configuration,
/// \param to configuration to project
///
/// \f[
/// \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);
protected:
/// Constructor
/// \param robot robot the constraint applies to.
/// \param errorThreshold norm of the value of the constraint under which
/// the constraint is considered satified,
/// \param maxIterations maximal number of iteration in the resolution of
/// the constraint.
ConfigProjector (const DevicePtr_t& robot, const std::string& name,
value_type errorThreshold, size_type maxIterations);
/// Store weak pointer to itself
void init (const ConfigProjectorPtr_t& self)
{
Constraint::init (self);
weak_ = self;
}
/// Numerically solve constraint
virtual bool impl_compute (Configuration_t& configuration);
/// Set locked degrees of freedom to their locked values
void computeLockedDofs (Configuration_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);
struct FunctionValueAndJacobian_t {
FunctionValueAndJacobian_t (DifferentiableFunctionPtr_t f,
vector_t v, matrix_t j): function (f),
value (v),
jacobian (j) {}
DifferentiableFunctionPtr_t function;
vector_t value;
matrix_t jacobian;
}; // struct FunctionValueAndJacobian_t
typedef std::vector < FunctionValueAndJacobian_t > NumericalConstraints_t;
void resize ();
void computeValueAndJacobian (const Configuration_t& configuration);
virtual void addLockedDof (const LockedDofPtr_t& lockedDof);
void computeIntervals ();
typedef std::list <LockedDofPtr_t> LockedDofs_t;
typedef std::vector < std::pair <size_type, size_type> >Intervals_t;
DevicePtr_t robot_;
NumericalConstraints_t constraints_;
LockedDofs_t lockedDofs_;
/// Intervals of non locked degrees of freedom
Intervals_t intervals_;
value_type squareErrorThreshold_;
size_type maxIterations_;
mutable vector_t value_;
/// Jacobian without locked degrees of freedom
mutable matrix_t reducedJacobian_;
mutable matrix_t reducedProjector_;
mutable vector_t toMinusFrom_;
mutable vector_t toMinusFromSmall_;
mutable vector_t projMinusFrom_;
mutable vector_t projMinusFromSmall_;
mutable vector_t dq_;
mutable vector_t dqSmall_;
size_type nbNonLockedDofs_;
ConfigProjectorWkPtr weak_;
}; // class ConfigProjector
} // namespace core
} // namespace hpp
#endif // HPP_CORE_CONFIG_PROJECTOR_HH
//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core 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-core 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-core If not, see
// <http://www.gnu.org/licenses/>.
#ifndef HPP_CORE_CONFIGURATION_SHOOTER_HH
# define HPP_CORE_CONFIGURATION_SHOOTER_HH
namespace hpp {
namespace core {
/// Abstraction of configuration shooter
///
/// Configuration shooters are used by random sampling algorithms to
/// generate new configurations
class HPP_CORE_DLLAPI ConfigurationShooter
{
public:
ConfigurationShooter ()
{
}
/// Shoot a random configuration
virtual ConfigurationPtr_t shoot () const = 0;
}; // class
} // namespace core
} // namespace hpp
#endif // HPP_CORE_CONFIGURATION_SHOOTER_HH
//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core 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-core 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-core If not, see
// <http://www.gnu.org/licenses/>.
#ifndef HPP_CORE_CONNECTED_COMPONENT_HH
# define HPP_CORE_CONNECTED_COMPONENT_HH
# include <hpp/core/fwd.hh>
# include <hpp/core/config.hh>
# include <hpp/core/node.hh>
namespace hpp {
namespace core {
/// Connected component
///
/// Set of nodes reachable from one another.
class HPP_CORE_DLLAPI ConnectedComponent {
public:
typedef std::list <NodePtr_t> Nodes_t;
static ConnectedComponentPtr_t create ()
{
ConnectedComponent* ptr = new ConnectedComponent ();
ConnectedComponentPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
/// Merge two connected components.
///
/// \param other connected component to merge into this one.
/// \note other will be empty after calling this method.
void merge (const ConnectedComponentPtr_t& other)
{
for (Nodes_t::iterator itNode = other->nodes_.begin ();
itNode != other->nodes_.end (); itNode++) {
(*itNode)->connectedComponent (weak_.lock ());
}
nodes_.splice (nodes_.end (), other->nodes_);
}
/// Add node in connected component
/// \param node node to add.
void addNode (const NodePtr_t& node)
{
nodes_.push_back (node);
}
/// Access to the nodes
const Nodes_t& nodes () const
{
return nodes_;
}
protected:
/// Constructor
ConnectedComponent () : nodes_ (), weak_ ()
{
}
void init (const ConnectedComponentPtr_t& shPtr){
weak_ = shPtr;
}
private:
Nodes_t nodes_;
ConnectedComponentWkPtr weak_;
}; // class ConnectedComponent
} // namespace core
} // namespace hpp
#endif // HPP_CORE_CONNECTED_COMPONENT_HH
//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core 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-core 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-core If not, see
// <http://www.gnu.org/licenses/>.
#ifndef HPP_CORE_CONSTRAINT_SET_HH
# define HPP_CORE_CONSTRAINT_SET_HH
# include <hpp/core/constraint.hh>
namespace hpp {
namespace core {
/// Set of constraints applicable to a robot configuration
///
/// \warning If the set is to contain a ConfigProjector and several
/// LockedDof instances, the configProjector should be inserted first
/// since following numerical projections might affect locked degrees of
/// freedom.
class HPP_CORE_DLLAPI ConstraintSet : public Constraint
{
public:
/// Return shared pointer to new object
static ConstraintSetPtr_t create (const DevicePtr_t& robot,
const std::string& name)
{
ConstraintSet* ptr = new ConstraintSet (robot, name);
ConstraintSetPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
/// Add a constraint to the set
void addConstraint (const ConstraintPtr_t& constraint)
{
constraint->addToConstraintSet (weak_.lock ());
}
/// Whether constraint set contains constraints of type LockedDof.
bool hasLockedDofs () const
{
return hasLockedDofs_;
}
/// Return pointer to config projector if any
const ConfigProjectorPtr_t& configProjector () const
{
return configProjector_;
}
protected:
typedef std::vector <ConstraintPtr_t> Constraints_t;
ConstraintSet (const DevicePtr_t& robot, const std::string& name);
/// Store weak pointer to itself.
void init (const ConstraintSetPtr_t& self)
{
Constraint::init (self);
weak_ = self;
}
virtual bool impl_compute (Configuration_t& configuration);
private:
virtual void addToConstraintSet (const ConstraintSetPtr_t& constraintSet)
{
for (Constraints_t::iterator itConstraint = constraints_.begin ();
itConstraint != constraints_.end (); itConstraint ++) {
(*itConstraint)->addToConstraintSet (constraintSet);
}
}
virtual void addLockedDof (const LockedDofPtr_t& lockedDof)
{
for (Constraints_t::iterator itConstraint = constraints_.begin ();
itConstraint != constraints_.end (); itConstraint ++) {
(*itConstraint)->addLockedDof (lockedDof);
}
}
virtual std::ostream& print (std::ostream& os) const
{
os << "Constraint set " << name () << ", contains" << std::endl;
for (Constraints_t::const_iterator itConstraint = constraints_.begin ();
itConstraint != constraints_.end (); itConstraint++) {
os << " " << **itConstraint << std::endl;
}
return os;
}
Constraints_t constraints_;
ConfigProjectorPtr_t configProjector_;
bool hasLockedDofs_;
ConstraintSetWkPtr weak_;
friend class LockedDof;
friend class Constraint;
friend class ConfigProjector;
}; // class ConstraintSet
} // namespace core
} // namespace hpp
#endif // HPP_CORE_CONSTRAINT_SET_HH
//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core 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-core 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