Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision
  • devel
  • master
  • pr/142
  • pr/163
  • pre-commit-ci-update-config
  • 4.5.0-rc
  • v1.0
  • v1.1.0
  • v4.0
  • v4.1
  • v4.10.1
  • v4.11.0
  • v4.12.0
  • v4.13.0
  • v4.14.0
  • v4.15.1
  • v4.2.0
  • v4.3.0
  • v4.4.0
  • v4.4.0-rc
  • v4.5.0
  • v4.6.0
  • v4.7.0
  • v4.8.0
  • v4.9.0
  • v5.0.0
  • v5.1.0
  • v5.2.0
  • v6.0.0
29 results

Target

Select target project
  • gsaurel/hpp-manipulation
  • humanoid-path-planner/hpp-manipulation
2 results
Select Git revision
  • devel
  • master
  • v1.0
  • v1.1.0
  • v4.0
  • v4.1
  • v4.10.1
  • v4.11.0
  • v4.12.0
  • v4.13.0
  • v4.14.0
  • v4.15.1
  • v4.2.0
  • v4.3.0
  • v4.4.0
  • v4.4.0-rc
  • v4.5.0
  • v4.7.0
  • v4.8.0
  • v4.9.0
20 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 2525 additions and 2090 deletions
include/hpp/manipulation/device.hh
View file @ 539aab52
......@@ -3,107 +3,110 @@
/// Authors: Florent Lamiraux, Joseph Mirabel
///
///
// 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.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 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/>.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_DEVICE_HH
# define HPP_MANIPULATION_DEVICE_HH
#define HPP_MANIPULATION_DEVICE_HH
# include <hpp/model/humanoid-robot.hh>
# include <hpp/core/container.hh>
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/config.hh"
#include <hpp/core/container.hh>
#include <hpp/manipulation/config.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/pinocchio/humanoid-robot.hh>
namespace hpp {
namespace manipulation {
/// Device with handles.
///
/// As a deriving class of hpp::model::HumanoidRobot,
/// it is compatible with hpp::model::urdf::loadHumanoidRobot
///
/// This class also contains model::Gripper, Handle and \ref JointAndTriangles_t
class HPP_MANIPULATION_DLLAPI Device :
public model::HumanoidRobot,
public core::Containers<
boost::mpl::vector < HandlePtr_t,
model::GripperPtr_t,
JointAndShapes_t,
model::JointVector_t> >
{
public:
typedef model::HumanoidRobot Parent_t;
typedef core::Containers<
boost::mpl::vector < HandlePtr_t,
model::GripperPtr_t,
JointAndShapes_t,
model::JointVector_t> > Containers_t;
/// Constructor
/// \param name of the new instance,
static DevicePtr_t create (const std::string& name)
{
Device* ptr = new Device (name);
DevicePtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
/// Print object in a stream
virtual std::ostream& print (std::ostream& os) const;
/// \name Collisions
/// \{
/// Cache joint vector
void prepareInsertRobot ()
{
didPrepare_ = true;
jointCache_ = getJointVector ();
}
/// Add collisions
/// between the joint vector cache initialized by prepareInsertRobot()
/// add the current Robot.
/// When creating a robot from several URDF files, this enables
/// collisions between joints from different files.
void didInsertRobot (const std::string& name);
/// \}
protected:
/// Constructor
/// \param name of the new instance,
/// \param robot Robots that manipulate objects,
/// \param objects Set of objects manipulated by the robot.
Device (const std::string& name) :
Parent_t (name), jointCache_ (), didPrepare_ (false)
{}
void init (const DeviceWkPtr_t& self)
{
Parent_t::init (self);
self_ = self;
}
private:
DeviceWkPtr_t self_;
model::JointVector_t jointCache_;
bool didPrepare_;
}; // class Device
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_DEVICE_HH
namespace manipulation {
/// Device with handles.
///
/// As a deriving class of hpp::pinocchio::HumanoidRobot,
/// it is compatible with hpp::pinocchio::urdf::loadHumanoidRobot
///
/// This class also contains pinocchio::Gripper, Handle and \ref
/// JointAndShapes_t
class HPP_MANIPULATION_DLLAPI Device : public pinocchio::HumanoidRobot {
public:
typedef pinocchio::HumanoidRobot Parent_t;
/// Constructor
/// \param name of the new instance,
static DevicePtr_t create(const std::string& name) {
Device* ptr = new Device(name);
DevicePtr_t shPtr(ptr);
ptr->init(shPtr);
return shPtr;
}
DevicePtr_t self() const { return self_.lock(); }
/// Print object in a stream
virtual std::ostream& print(std::ostream& os) const;
void setRobotRootPosition(const std::string& robotName,
const Transform3s& positionWRTParentJoint);
virtual pinocchio::DevicePtr_t clone() const;
std::vector<std::string> robotNames() const;
FrameIndices_t robotFrames(const std::string& robotName) const;
void removeJoints(const std::vector<std::string>& jointNames,
Configuration_t referenceConfig);
core::Container<HandlePtr_t> handles;
core::Container<GripperPtr_t> grippers;
core::Container<JointAndShapes_t> jointAndShapes;
protected:
/// Constructor
/// \param name of the new instance,
/// \param robot Robots that manipulate objects,
/// \param objects Set of objects manipulated by the robot.
Device(const std::string& name) : Parent_t(name) {}
void init(const DeviceWkPtr_t& self) {
Parent_t::init(self);
self_ = self;
}
void initCopy(const DeviceWkPtr_t& self, const Device& other) {
Parent_t::initCopy(self, other);
self_ = self;
}
/// For serialization only
Device() {}
private:
DeviceWkPtr_t self_;
HPP_SERIALIZABLE();
}; // class Device
} // namespace manipulation
} // namespace hpp
BOOST_CLASS_EXPORT_KEY(hpp::manipulation::Device)
#endif // HPP_MANIPULATION_DEVICE_HH
include/hpp/manipulation/fwd.hh
View file @ 539aab52
......@@ -3,132 +3,173 @@
// Authors: Florent Lamiraux, Joseph Mirabel
//
//
// 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.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 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/>.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_FWD_HH
# define HPP_MANIPULATION_FWD_HH
#define HPP_MANIPULATION_FWD_HH
# include <map>
# include <hpp/core/fwd.hh>
# include <hpp/fcl/shape/geometric_shapes.h>
# include <hpp/manipulation/deprecated.hh>
#include <hpp/core/fwd.hh>
#include <map>
namespace hpp {
namespace manipulation {
HPP_PREDEF_CLASS (Device);
typedef boost::shared_ptr <Device> DevicePtr_t;
typedef boost::shared_ptr <const Device> DeviceConstPtr_t;
typedef model::Joint Joint;
typedef model::JointPtr_t JointPtr_t;
typedef model::JointConstPtr_t JointConstPtr_t;
typedef model::Transform3f Transform3f;
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 model::GripperPtr_t GripperPtr_t;
HPP_PREDEF_CLASS (AxialHandle);
typedef boost::shared_ptr <AxialHandle> AxialHandlePtr_t;
HPP_PREDEF_CLASS (Handle);
typedef boost::shared_ptr <Handle> HandlePtr_t;
HPP_PREDEF_CLASS (Object);
typedef boost::shared_ptr <Object> ObjectPtr_t;
typedef boost::shared_ptr <const Object> ObjectConstPtr_t;
HPP_PREDEF_CLASS (ProblemSolver);
typedef ProblemSolver* ProblemSolverPtr_t;
HPP_PREDEF_CLASS (Problem);
typedef Problem* ProblemPtr_t;
HPP_PREDEF_CLASS (Roadmap);
typedef boost::shared_ptr <Roadmap> RoadmapPtr_t;
HPP_PREDEF_CLASS (RoadmapNode);
typedef RoadmapNode* RoadmapNodePtr_t;
HPP_PREDEF_CLASS (ConnectedComponent);
typedef boost::shared_ptr<ConnectedComponent> ConnectedComponentPtr_t;
HPP_PREDEF_CLASS (WeighedDistance);
typedef boost::shared_ptr<WeighedDistance> WeighedDistancePtr_t;
typedef constraints::RelativeOrientation RelativeOrientation;
typedef constraints::RelativePosition RelativePosition;
typedef constraints::RelativeOrientationPtr_t RelativeOrientationPtr_t;
typedef constraints::RelativePositionPtr_t RelativePositionPtr_t;
typedef constraints::RelativeTransformation RelativeTransformation;
typedef constraints::RelativeTransformationPtr_t
RelativeTransformationPtr_t;
typedef model::value_type value_type;
typedef model::size_type size_type;
typedef model::Transform3f Transform3f;
typedef model::vector_t vector_t;
typedef model::vectorIn_t vectorIn_t;
typedef model::vectorOut_t vectorOut_t;
HPP_PREDEF_CLASS (ManipulationPlanner);
typedef boost::shared_ptr < ManipulationPlanner > ManipulationPlannerPtr_t;
HPP_PREDEF_CLASS (GraphPathValidation);
typedef boost::shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
HPP_PREDEF_CLASS (GraphSteeringMethod);
typedef boost::shared_ptr < GraphSteeringMethod > GraphSteeringMethodPtr_t;
typedef core::PathOptimizer PathOptimizer;
typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
HPP_PREDEF_CLASS (GraphOptimizer);
typedef boost::shared_ptr < GraphOptimizer > GraphOptimizerPtr_t;
HPP_PREDEF_CLASS (GraphNodeOptimizer);
typedef boost::shared_ptr < GraphNodeOptimizer > GraphNodeOptimizerPtr_t;
typedef core::PathProjectorPtr_t PathProjectorPtr_t;
namespace manipulation {
HPP_PREDEF_CLASS(Device);
typedef shared_ptr<Device> DevicePtr_t;
typedef shared_ptr<const Device> DeviceConstPtr_t;
typedef pinocchio::Joint Joint;
typedef pinocchio::JointPtr_t JointPtr_t;
typedef pinocchio::JointIndex JointIndex;
typedef std::vector<JointIndex> JointIndices_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;
typedef pinocchio::GripperPtr_t GripperPtr_t;
typedef pinocchio::LiegroupElement LiegroupElement;
typedef pinocchio::LiegroupSpace LiegroupSpace;
typedef pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t;
HPP_PREDEF_CLASS(AxialHandle);
typedef shared_ptr<AxialHandle> AxialHandlePtr_t;
HPP_PREDEF_CLASS(Handle);
typedef shared_ptr<Handle> HandlePtr_t;
HPP_PREDEF_CLASS(Object);
typedef shared_ptr<Object> ObjectPtr_t;
typedef shared_ptr<const Object> ObjectConstPtr_t;
HPP_PREDEF_CLASS(ProblemSolver);
typedef ProblemSolver* ProblemSolverPtr_t;
HPP_PREDEF_CLASS(Problem);
typedef shared_ptr<Problem> ProblemPtr_t;
typedef shared_ptr<const Problem> ProblemConstPtr_t;
HPP_PREDEF_CLASS(Roadmap);
typedef shared_ptr<Roadmap> RoadmapPtr_t;
HPP_PREDEF_CLASS(RoadmapNode);
typedef RoadmapNode* RoadmapNodePtr_t;
typedef std::vector<RoadmapNodePtr_t> RoadmapNodes_t;
HPP_PREDEF_CLASS(ConnectedComponent);
typedef shared_ptr<ConnectedComponent> ConnectedComponentPtr_t;
HPP_PREDEF_CLASS(LeafConnectedComp);
typedef shared_ptr<LeafConnectedComp> LeafConnectedCompPtr_t;
typedef shared_ptr<const LeafConnectedComp> LeafConnectedCompConstPtr_t;
typedef std::set<LeafConnectedCompPtr_t> LeafConnectedComps_t;
HPP_PREDEF_CLASS(WeighedLeafConnectedComp);
typedef shared_ptr<WeighedLeafConnectedComp> WeighedLeafConnectedCompPtr_t;
HPP_PREDEF_CLASS(WeighedDistance);
typedef shared_ptr<WeighedDistance> WeighedDistancePtr_t;
typedef constraints::RelativeOrientation RelativeOrientation;
typedef constraints::RelativePosition RelativePosition;
typedef constraints::RelativeOrientationPtr_t RelativeOrientationPtr_t;
typedef constraints::RelativePositionPtr_t RelativePositionPtr_t;
typedef constraints::RelativeTransformation RelativeTransformation;
typedef constraints::RelativeTransformationR3xSO3 RelativeTransformationR3xSO3;
typedef constraints::RelativeTransformationPtr_t RelativeTransformationPtr_t;
typedef core::value_type value_type;
typedef core::size_type size_type;
typedef core::Transform3s Transform3s;
typedef core::vector_t vector_t;
typedef core::vectorIn_t vectorIn_t;
typedef core::vectorOut_t vectorOut_t;
HPP_PREDEF_CLASS(ManipulationPlanner);
typedef shared_ptr<ManipulationPlanner> ManipulationPlannerPtr_t;
namespace pathPlanner {
HPP_PREDEF_CLASS(EndEffectorTrajectory);
typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
HPP_PREDEF_CLASS(StatesPathFinder);
typedef shared_ptr<StatesPathFinder> StatesPathFinderPtr_t;
HPP_PREDEF_CLASS(InStatePath);
typedef shared_ptr<InStatePath> InStatePathPtr_t;
HPP_PREDEF_CLASS(StateShooter);
typedef shared_ptr<StateShooter> StateShooterPtr_t;
HPP_PREDEF_CLASS(TransitionPlanner);
typedef shared_ptr<TransitionPlanner> TransitionPlannerPtr_t;
} // namespace pathPlanner
HPP_PREDEF_CLASS(GraphPathValidation);
typedef shared_ptr<GraphPathValidation> GraphPathValidationPtr_t;
HPP_PREDEF_CLASS(SteeringMethod);
typedef shared_ptr<SteeringMethod> SteeringMethodPtr_t;
namespace steeringMethod {
HPP_PREDEF_CLASS(EndEffectorTrajectory);
typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
} // namespace steeringMethod
typedef core::PathOptimizer PathOptimizer;
typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
HPP_PREDEF_CLASS(GraphOptimizer);
typedef shared_ptr<GraphOptimizer> GraphOptimizerPtr_t;
HPP_PREDEF_CLASS(GraphNodeOptimizer);
typedef shared_ptr<GraphNodeOptimizer> GraphNodeOptimizerPtr_t;
typedef core::PathProjectorPtr_t PathProjectorPtr_t;
typedef std::vector<pinocchio::DevicePtr_t> Devices_t;
typedef std::vector<ObjectPtr_t> Objects_t;
typedef core::Constraint Constraint;
typedef core::ConstraintPtr_t ConstraintPtr_t;
typedef constraints::Explicit Explicit;
typedef constraints::ExplicitPtr_t ExplicitPtr_t;
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
typedef constraints::LockedJoint LockedJoint;
typedef constraints::LockedJointPtr_t LockedJointPtr_t;
typedef hpp::core::ComparisonTypes_t ComparisonTypes_t;
typedef core::ConfigProjector ConfigProjector;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
HPP_PREDEF_CLASS(ConstraintSet);
typedef shared_ptr<ConstraintSet> ConstraintSetPtr_t;
typedef core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
typedef core::ConfigurationShooter ConfigurationShooter;
typedef core::ConfigurationShooterPtr_t ConfigurationShooterPtr_t;
typedef core::ValidationReport ValidationReport;
typedef core::NumericalConstraints_t NumericalConstraints_t;
typedef core::PathValidationPtr_t PathValidationPtr_t;
typedef core::PathValidationReportPtr_t PathValidationReportPtr_t;
typedef core::matrix_t matrix_t;
typedef core::matrixIn_t matrixIn_t;
typedef core::matrixOut_t matrixOut_t;
typedef core::size_type size_type;
typedef core::value_type value_type;
typedef core::vector3_t vector3_t;
typedef core::matrix3_t matrix3_t;
typedef std::vector <model::DevicePtr_t> Devices_t;
typedef std::vector <ObjectPtr_t> Objects_t;
typedef std::map <JointConstPtr_t, JointPtr_t> JointMap_t;
typedef core::Constraint Constraint;
typedef core::ConstraintPtr_t ConstraintPtr_t;
typedef core::LockedJoint LockedJoint;
typedef core::LockedJointPtr_t LockedJointPtr_t;
typedef core::NumericalConstraint NumericalConstraint;
typedef core::NumericalConstraintPtr_t NumericalConstraintPtr_t;
typedef core::ConfigProjector ConfigProjector;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
HPP_PREDEF_CLASS (ConstraintSet);
typedef boost::shared_ptr <ConstraintSet> ConstraintSetPtr_t;
typedef core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
typedef core::ConfigurationShooter ConfigurationShooter;
typedef core::ConfigurationShooterPtr_t ConfigurationShooterPtr_t;
typedef core::ValidationReport ValidationReport;
typedef core::PathValidationReportPtr_t PathValidationReportPtr_t;
typedef core::matrix_t matrix_t;
typedef core::matrixIn_t matrixIn_t;
typedef core::matrixOut_t matrixOut_t;
typedef core::size_type size_type;
typedef core::value_type value_type;
typedef core::vector3_t vector3_t;
typedef core::Shape_t Shape_t;
typedef core::JointAndShape_t JointAndShape_t;
typedef core::JointAndShapes_t JointAndShapes_t;
typedef std::list < NumericalConstraintPtr_t > NumericalConstraints_t;
typedef std::pair< GripperPtr_t, HandlePtr_t> Grasp_t;
typedef boost::shared_ptr <Grasp_t> GraspPtr_t;
typedef std::map <NumericalConstraintPtr_t, GraspPtr_t> GraspsMap_t;
typedef std::list<std::string> StringList_t;
typedef std::vector<std::string> Strings_t;
typedef std::vector<core::vector3_t> Shape_t;
typedef std::pair <JointPtr_t, Shape_t> JointAndShape_t;
typedef std::list <JointAndShape_t> JointAndShapes_t;
typedef std::list <std::string> StringList_t;
typedef HPP_MANIPULATION_DEPRECATED Shape_t Triangle;
typedef HPP_MANIPULATION_DEPRECATED JointAndShape_t JointAndTriangle_t;
typedef HPP_MANIPULATION_DEPRECATED JointAndShapes_t JointAndTriangles_t;
namespace pathOptimization {
HPP_PREDEF_CLASS(SmallSteps);
typedef shared_ptr<SmallSteps> SmallStepsPtr_t;
HPP_PREDEF_CLASS(Keypoints);
typedef shared_ptr<Keypoints> KeypointsPtr_t;
} // namespace pathOptimization
namespace pathOptimization {
HPP_PREDEF_CLASS (SmallSteps);
typedef boost::shared_ptr < SmallSteps > SmallStepsPtr_t;
} // namespace pathOptimization
} // namespace manipulation
} // namespace hpp
namespace problemTarget {
HPP_PREDEF_CLASS(State);
typedef shared_ptr<State> StatePtr_t;
} // namespace problemTarget
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_FWD_HH
#endif // HPP_MANIPULATION_FWD_HH
include/hpp/manipulation/graph-node-optimizer.hh
View file @ 539aab52
// Copyright (c) 2015, 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.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 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/>.
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
# define HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
# include <hpp/core/path.hh>
# include <hpp/core/path-vector.hh>
# include <hpp/core/path-optimizer.hh>
# include <hpp/core/problem.hh>
# include <hpp/core/problem-solver.hh>
#define HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
# include <hpp/manipulation/fwd.hh>
# include <hpp/manipulation/graph/fwd.hh>
# include <hpp/manipulation/config.hh>
# include <hpp/manipulation/constraint-set.hh>
#include <hpp/core/path-optimizer.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/path.hh>
#include <hpp/core/problem-solver.hh>
#include <hpp/core/problem.hh>
#include <hpp/manipulation/config.hh>
#include <hpp/manipulation/constraint-set.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/manipulation/graph/fwd.hh>
namespace hpp {
namespace manipulation {
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t;
namespace manipulation {
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t;
/// \addtogroup path_optimization
/// \{
/// \addtogroup path_optimization
/// \{
/// Path optimizer that recompute the edge parameter of the constraints
///
/// This class encapsulates another path optimizer class. This optimizer
/// calls the inner optimizer on every subpaths with the same set of
/// constraints.
class HPP_MANIPULATION_DLLAPI GraphNodeOptimizer : public PathOptimizer
{
public:
static GraphNodeOptimizerPtr_t create (const core::Problem& problem);
/// Path optimizer that recompute the edge parameter of the constraints
///
/// This class encapsulates another path optimizer class. This optimizer
/// calls the inner optimizer on every subpaths with the same set of
/// constraints.
class HPP_MANIPULATION_DLLAPI GraphNodeOptimizer : public PathOptimizer {
public:
static GraphNodeOptimizerPtr_t create(const core::ProblemConstPtr_t& problem);
virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
protected:
/// Constructor
GraphNodeOptimizer (const core::Problem& problem) :
PathOptimizer (problem)
{}
protected:
/// Constructor
GraphNodeOptimizer(const core::ProblemConstPtr_t& problem)
: PathOptimizer(problem) {}
private:
};
/// \}
} // namespace manipulation
} // namespace hpp
private:
};
/// \}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
#endif // HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
include/hpp/manipulation/graph-optimizer.hh
View file @ 539aab52
// Copyright (c) 2015, 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/>.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPHOPTIMIZER_HH
# define HPP_MANIPULATION_GRAPHOPTIMIZER_HH
#define HPP_MANIPULATION_GRAPHOPTIMIZER_HH
# include <hpp/core/path.hh>
# include <hpp/core/path-vector.hh>
# include <hpp/core/path-optimizer.hh>
# include <hpp/core/problem.hh>
# include <hpp/core/problem-solver.hh>
# include <hpp/manipulation/fwd.hh>
# include <hpp/manipulation/graph/fwd.hh>
# include <hpp/manipulation/config.hh>
# include <hpp/manipulation/constraint-set.hh>
#include <hpp/core/path-optimizer.hh>
#include <hpp/core/problem-solver.hh> // PathOptimizerBuilder_t
#include <hpp/manipulation/config.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/manipulation/graph/fwd.hh>
namespace hpp {
namespace manipulation {
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t;
/// \addtogroup path_optimization
/// \{
/// Path optimizer for paths created with the constraint graph
///
/// This class encapsulates another path optimizer class. This optimizer
/// calls the inner optimizer on every subpaths with the same set of
/// constraints.
class HPP_MANIPULATION_DLLAPI GraphOptimizer : public PathOptimizer
{
public:
typedef core::PathOptimizerBuilder_t PathOptimizerBuilder_t;
template <typename TraitsOrInnerType>
static GraphOptimizerPtr_t create (const core::Problem& problem);
virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
/// Get the encapsulated optimizer
const PathOptimizerPtr_t& innerOptimizer ()
{
return pathOptimizer_;
}
protected:
/// Constructor
GraphOptimizer (const core::Problem& problem, PathOptimizerBuilder_t factory) :
PathOptimizer (problem), factory_ (factory), pathOptimizer_ ()
{}
private:
PathOptimizerBuilder_t factory_;
/// The encapsulated PathOptimizer
PathOptimizerPtr_t pathOptimizer_;
};
/// \}
/// Member function definition
template <typename TraitsOrInnerType>
GraphOptimizerPtr_t GraphOptimizer::create
(const core::Problem& problem)
{
return GraphOptimizerPtr_t (
new GraphOptimizer (problem, TraitsOrInnerType::create)
);
}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPHOPTIMIZER_HH
namespace manipulation {
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t;
/// \addtogroup path_optimization
/// \{
/// Path optimizer for paths created with the constraint graph
///
/// This class encapsulates another path optimizer class. This optimizer
/// calls the inner optimizer on every subpaths with the same set of
/// constraints.
class HPP_MANIPULATION_DLLAPI GraphOptimizer : public PathOptimizer {
public:
typedef core::PathOptimizerBuilder_t PathOptimizerBuilder_t;
template <typename TraitsOrInnerType>
static GraphOptimizerPtr_t create(const core::ProblemConstPtr_t& problem);
virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
/// Get the encapsulated optimizer
const PathOptimizerPtr_t& innerOptimizer() { return pathOptimizer_; }
protected:
/// Constructor
GraphOptimizer(const core::ProblemConstPtr_t& problem,
PathOptimizerBuilder_t factory)
: PathOptimizer(problem), factory_(factory), pathOptimizer_() {}
private:
PathOptimizerBuilder_t factory_;
/// The encapsulated PathOptimizer
PathOptimizerPtr_t pathOptimizer_;
};
/// \}
/// Member function definition
template <typename TraitsOrInnerType>
GraphOptimizerPtr_t GraphOptimizer::create(
const core::ProblemConstPtr_t& problem) {
return GraphOptimizerPtr_t(
new GraphOptimizer(problem, TraitsOrInnerType::create));
}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPHOPTIMIZER_HH
include/hpp/manipulation/graph-path-validation.hh
View file @ 539aab52
// 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/>.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
# define HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
# include <hpp/core/path.hh>
# include <hpp/core/path-vector.hh>
# include <hpp/core/path-validation.hh>
#define HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/graph.hh"
# include "hpp/manipulation/graph/node.hh"
#include <hpp/core/obstacle-user.hh>
#include <hpp/core/path-validation.hh>
#include <hpp/manipulation/config.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/manipulation/graph/fwd.hh>
namespace hpp {
namespace manipulation {
using hpp::core::PathValidation;
using hpp::core::PathValidationPtr_t;
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t;
using graph::GraphPtr_t;
/// \addtogroup validation
/// \{
/// Path validation for a constraint graph
///
/// This class encapsulates another path validation class.
/// The encapsulated path validation is responsible for collision
/// checking, whereas this class checks if a path is valid regarding
/// the constraint graph.
class HPP_MANIPULATION_DLLAPI GraphPathValidation : public PathValidation
{
public:
/// Check that path is valid regarding the constraint graph.
///
/// \param path the path to check for validity,
/// \param reverse if true check from the end,
/// \retval the extracted valid part of the path, pointer to path if
/// path is valid,
/// \retval report information about the validation process. unused in
/// this case,
/// \return whether the whole path is valid.
///
/// \notice Call the encapsulated PathValidation::validate.
virtual bool validate (const PathPtr_t& path, bool reverse,
PathPtr_t& validPart,
PathValidationReportPtr_t& report);
/// Set the encapsulated path validator.
void innerValidation (const PathValidationPtr_t& pathValidation)
{
pathValidation_ = pathValidation;
}
/// Get the encapsulated path validator.
const PathValidationPtr_t& innerValidation ()
{
return pathValidation_;
}
/// Set the graph of constraints
void constraintGraph (const graph::GraphPtr_t& graph)
{
constraintGraph_ = graph;
}
/// Get the graph of constraints
graph::GraphPtr_t constraintGraph () const
{
return constraintGraph_;
}
/// Create a new instance of this class.
/// \param pathValidation a PathValidation that is responsible for collision
// checking.
// \param graph A pointer to the constraint graph.
static GraphPathValidationPtr_t create (
const PathValidationPtr_t& pathValidation);
template <typename T>
static GraphPathValidationPtr_t create (
const model::DevicePtr_t& robot, const value_type& stepSize);
void addObstacle (const hpp::core::CollisionObjectPtr_t&);
/// Remove a collision pair between a joint and an obstacle
/// \param joint the joint that holds the inner objects,
/// \param obstacle the obstacle to remove.
/// \notice collision configuration validation needs to know about
/// obstacles. This virtual method does nothing for configuration
/// validation methods that do not care about obstacles.
virtual void removeObstacleFromJoint (const JointPtr_t& joint,
const model::CollisionObjectPtr_t& obstacle)
{
assert (pathValidation_);
pathValidation_->removeObstacleFromJoint (joint, obstacle);
}
protected:
/// Constructor
GraphPathValidation (const PathValidationPtr_t& pathValidation);
private:
/// Do validation regarding the constraint graph for PathVector
bool impl_validate (const PathVectorPtr_t& path, bool reverse,
PathPtr_t& validPart, PathValidationReportPtr_t& report);
/// Do validation regarding the constraint graph for Path
bool impl_validate (const PathPtr_t& path, bool reverse,
PathPtr_t& validPart, PathValidationReportPtr_t& report);
/// The encapsulated PathValidation.
PathValidationPtr_t pathValidation_;
/// Pointer to the constraint graph.
GraphPtr_t constraintGraph_;
};
template <typename T>
GraphPathValidationPtr_t GraphPathValidation::create
(const model::DevicePtr_t& robot, const value_type& stepSize)
{
return GraphPathValidation::create (T::create (robot, stepSize));
}
/// \}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
namespace manipulation {
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathValidation;
using hpp::core::PathValidationPtr_t;
using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t;
/// \addtogroup validation
/// \{
/// Path validation for a constraint graph
///
/// This class encapsulates another path validation class.
/// The encapsulated path validation is responsible for collision
/// checking, whereas this class checks if a path is valid regarding
/// the constraint graph.
class HPP_MANIPULATION_DLLAPI GraphPathValidation
: public PathValidation,
public core::ObstacleUserInterface {
public:
/// Check that path is valid regarding the constraint graph.
///
/// \param path the path to check for validity,
/// \param reverse if true check from the end,
/// \retval the extracted valid part of the path, pointer to path if
/// path is valid,
/// \retval report information about the validation process. unused in
/// this case,
/// \return whether the whole path is valid.
///
/// \notice Call the encapsulated PathValidation::validate.
virtual bool validate(const PathPtr_t& path, bool reverse,
PathPtr_t& validPart,
PathValidationReportPtr_t& report);
/// Set the encapsulated path validator.
void innerValidation(const PathValidationPtr_t& pathValidation) {
pathValidation_ = pathValidation;
}
/// Get the encapsulated path validator.
const PathValidationPtr_t& innerValidation() { return pathValidation_; }
/// Set the graph of constraints
void constraintGraph(const graph::GraphPtr_t& graph) {
constraintGraph_ = graph;
}
/// Get the graph of constraints
graph::GraphPtr_t constraintGraph() const { return constraintGraph_; }
/// Create a new instance of this class.
/// \param pathValidation a PathValidation that is responsible for collision
// checking.
// \param graph A pointer to the constraint graph.
static GraphPathValidationPtr_t create(
const PathValidationPtr_t& pathValidation);
template <typename T>
static GraphPathValidationPtr_t create(const pinocchio::DevicePtr_t& robot,
const value_type& stepSize);
/// Add obstacle in the environment
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::addObstacle in case of success.
void addObstacle(const hpp::core::CollisionObjectConstPtr_t& object) {
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->addObstacle(object);
}
/// Remove a collision pair between a joint and an obstacle
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::removeObstacleFromJoint in case
/// of success.
void removeObstacleFromJoint(
const JointPtr_t& joint,
const pinocchio::CollisionObjectConstPtr_t& obstacle) {
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->removeObstacleFromJoint(joint, obstacle);
}
/// \brief Filter collision pairs.
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::filterCollisionPairs in case of
/// success.
void filterCollisionPairs(
const core::RelativeMotion::matrix_type& relMotion) {
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->filterCollisionPairs(relMotion);
}
/// Set different security margins for collision pairs
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
/// success.
void setSecurityMargins(const matrix_t& securityMargins) {
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->setSecurityMargins(securityMargins);
}
/// \copydoc hpp::core::ObstacleUserInterface::setSecurityMarginBetweenBodies
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
/// success.
void setSecurityMarginBetweenBodies(const std::string& body_a,
const std::string& body_b,
const value_type& margin) {
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->setSecurityMarginBetweenBodies(body_a, body_b, margin);
}
protected:
/// Constructor
GraphPathValidation(const PathValidationPtr_t& pathValidation);
private:
/// Do validation regarding the constraint graph for PathVector
bool impl_validate(const PathVectorPtr_t& path, bool reverse,
PathPtr_t& validPart, PathValidationReportPtr_t& report);
/// Do validation regarding the constraint graph for Path
bool impl_validate(const PathPtr_t& path, bool reverse, PathPtr_t& validPart,
PathValidationReportPtr_t& report);
/// The encapsulated PathValidation.
PathValidationPtr_t pathValidation_;
/// Pointer to the constraint graph.
graph::GraphPtr_t constraintGraph_;
};
template <typename T>
GraphPathValidationPtr_t GraphPathValidation::create(
const pinocchio::DevicePtr_t& robot, const value_type& stepSize) {
return GraphPathValidation::create(T::create(robot, stepSize));
}
/// \}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
include/hpp/manipulation/graph-steering-method.hh deleted 100644 → 0
View file @ 0e448947
// 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/>.
#ifndef HPP_MANIPULATION_GRAPH_STEERING_METHOD_HH
# define HPP_MANIPULATION_GRAPH_STEERING_METHOD_HH
# include <hpp/manipulation/config.hh>
# include "hpp/core/problem-solver.hh" // SteeringMethodBuilder_t
# include <hpp/core/steering-method.hh>
# include <hpp/core/weighed-distance.hh>
# include <hpp/model/device.hh>
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
# include "hpp/manipulation/problem.hh"
namespace hpp {
namespace manipulation {
using core::SteeringMethod;
using core::PathPtr_t;
/// \addtogroup steering_method
/// \{
class HPP_MANIPULATION_DLLAPI GraphSteeringMethod : public SteeringMethod
{
typedef core::SteeringMethodBuilder_t SteeringMethodBuilder_t;
public:
/// Create instance and return shared pointer
/// \warning core::ProblemPtr_t will be casted to ProblemPtr_t
static GraphSteeringMethodPtr_t create
(const core::ProblemPtr_t& problem);
template <typename T>
static GraphSteeringMethodPtr_t create
(const core::ProblemPtr_t& problem);
/// Create instance and return shared pointer
static GraphSteeringMethodPtr_t create (const ProblemPtr_t& problem);
/// Create copy and return shared pointer
static GraphSteeringMethodPtr_t createCopy
(const GraphSteeringMethodPtr_t& other);
/// Copy instance and return shared pointer
virtual core::SteeringMethodPtr_t copy () const
{
return createCopy (weak_.lock ());
}
const core::SteeringMethodPtr_t& innerSteeringMethod () const
{
return steeringMethod_;
}
void innerSteeringMethod (const core::SteeringMethodPtr_t& sm)
{
steeringMethod_ = sm;
}
protected:
/// Constructor
GraphSteeringMethod (const ProblemPtr_t& problem);
/// Copy constructor
GraphSteeringMethod (const GraphSteeringMethod&);
virtual PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
void init (GraphSteeringMethodWkPtr_t weak)
{
core::SteeringMethod::init (weak);
weak_ = weak;
}
private:
/// A pointer to the problem
ProblemPtr_t problem_;
/// Weak pointer to itself
GraphSteeringMethodWkPtr_t weak_;
/// The encapsulated steering method
core::SteeringMethodPtr_t steeringMethod_;
};
template <typename T>
GraphSteeringMethodPtr_t GraphSteeringMethod::create
(const core::ProblemPtr_t& problem)
{
GraphSteeringMethodPtr_t gsm = GraphSteeringMethod::create (problem);
gsm->innerSteeringMethod (T::create (problem));
return gsm;
}
/// \}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_STEERING_METHOD_HH
include/hpp/manipulation/graph/dot.hh
View file @ 539aab52
// 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.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 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/>.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_DOT_HH
# define HPP_MANIPULATION_GRAPH_DOT_HH
#define HPP_MANIPULATION_GRAPH_DOT_HH
# include <ostream>
# include <sstream>
# include <map>
# include <list>
#include <list>
#include <map>
#include <ostream>
#include <sstream>
namespace hpp {
namespace manipulation {
namespace graph {
namespace dot {
namespace manipulation {
namespace graph {
namespace dot {
struct DrawingAttributes {
typedef std::pair <std::string, std::string> Pair;
typedef std::map <std::string, std::string> Map;
struct DrawingAttributes {
typedef std::pair<std::string, std::string> Pair;
typedef std::map<std::string, std::string> Map;
std::string separator, openSection, closeSection;
Map attr;
std::string separator, openSection, closeSection;
Map attr;
inline void insertWithQuote (const std::string& K, const std::string& V) {
attr.insert (Pair (K, "\"" + V + "\""));
}
inline void insert (const std::string& K, const std::string& V) {
attr.insert (Pair (K, V));
}
std::string& operator [] (const std::string& K) {
return attr [K];
}
DrawingAttributes () :
separator (", "), openSection ("["), closeSection ("]"),
attr () {};
};
inline void insertWithQuote(const std::string& K, const std::string& V) {
attr.insert(Pair(K, "\"" + V + "\""));
}
inline void insert(const std::string& K, const std::string& V) {
attr.insert(Pair(K, V));
}
std::string& operator[](const std::string& K) { return attr[K]; }
DrawingAttributes()
: separator(", "), openSection("["), closeSection("]"), attr() {};
};
struct Tooltip {
static const std::string tooltipendl;
typedef std::list <std::string> TooltipLineVector;
TooltipLineVector v;
struct Tooltip {
static const std::string tooltipendl;
typedef std::list<std::string> TooltipLineVector;
TooltipLineVector v;
Tooltip () : v() {};
inline std::string toStr () const {
std::stringstream ss;
size_t i = v.size ();
for (TooltipLineVector::const_iterator
it = v.begin (); it != v.end (); ++it ) {
ss << *it;
i--;
if (i > 0) ss << tooltipendl;
}
return ss.str ();
}
inline void addLine (const std::string& l) {
v.push_back (l);
}
};
Tooltip() : v() {};
inline std::string toStr() const {
std::stringstream ss;
size_t i = v.size();
for (TooltipLineVector::const_iterator it = v.begin(); it != v.end();
++it) {
ss << *it;
i--;
if (i > 0) ss << tooltipendl;
}
return ss.str();
}
inline void addLine(const std::string& l) { v.push_back(l); }
};
std::ostream& insertComments (std::ostream& os, const std::string& c);
std::ostream& insertComments(std::ostream& os, const std::string& c);
std::ostream& operator<< (std::ostream& os, const DrawingAttributes& da);
} // namespace dot
} // namespace graph
} // namespace manipulation
} // namespace hpp
std::ostream& operator<<(std::ostream& os, const DrawingAttributes& da);
} // namespace dot
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_DOT_HH
#endif // HPP_MANIPULATION_GRAPH_DOT_HH
include/hpp/manipulation/graph/edge.hh
View file @ 539aab52
// 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.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 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/>.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_EDGE_HH
# define HPP_MANIPULATION_GRAPH_EDGE_HH
#define HPP_MANIPULATION_GRAPH_EDGE_HH
#include <hpp/core/constraint-set.hh>
#include <hpp/core/steering-method.hh>
#include <hpp/core/path.hh>
#include <hpp/core/relative-motion.hh>
#include <hpp/core/steering-method.hh>
#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/graph.hh"
namespace hpp {
namespace manipulation {
namespace graph {
/// Cache mechanism that enable const-correctness of member functions.
template <typename C>
class HPP_MANIPULATION_LOCAL Cache
{
public:
void set (const C& c)
{
c_ = c;
}
operator bool() const
{
return (bool)c_;
}
const C& get () const
{
return c_;
}
private:
C c_;
};
/// \addtogroup constraint_graph
/// \{
/// Abstract class representing representing the link between two nodes.
class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent
{
public:
/// Destructor
virtual ~Edge ();
/// Create a new empty Edge.
static EdgePtr_t create
(const std::string& name,
const GraphWkPtr_t& graph,
const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
virtual bool applyConstraints (core::NodePtr_t nnear, ConfigurationOut_t q) const;
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1,
ConfigurationIn_t q2) const;
/// Get the destination
NodePtr_t to () const;
/// Get the origin
NodePtr_t from () const;
/// Get the node in which path is.
NodePtr_t node () const
{
return node_.lock();
}
void node (NodePtr_t node)
{
node_ = node;
}
/// \deprecated use node(NodePtr_t) instead.
void isInNodeFrom (bool iinf) HPP_MANIPULATION_DEPRECATED
{
if (iinf) node_ = from_;
else node_ = to_;
}
/// \deprecated see NodePtr_t node() const
bool isInNodeFrom () const HPP_MANIPULATION_DEPRECATED
{
return node_.lock() == from_.lock();
}
/// Get steering method associated to the edge.
const core::SteeringMethodPtr_t& steeringMethod () const
{
return steeringMethod_->get();
}
/// Get path validation associated to the edge.
const core::PathValidationPtr_t& pathValidation () const
{
return pathValidation_->get();
}
/// Get direction of the path compare to the edge
/// \return true is reverse
virtual bool direction (const core::PathPtr_t& path) const;
/// Populate a ConfigProjector with constraints required to generate
/// a path at the intersection of two edges.
virtual bool intersectionConstraint (const EdgePtr_t& other,
ConfigProjectorPtr_t projector) const;
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
/// Constraint to project onto the same leaf as config.
/// \return The initialized projector.
ConstraintSetPtr_t configConstraint() const;
void setShort (bool isShort) {
isShort_ = isShort;
}
bool isShort () const {
return isShort_;
}
protected:
/// Initialization of the object.
void init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
/// Constructor
Edge (const std::string& name);
/// Constraint to project a path.
/// \return The initialized constraint.
ConstraintSetPtr_t pathConstraint() const;
virtual ConstraintSetPtr_t buildConfigConstraint() const;
virtual ConstraintSetPtr_t buildPathConstraint() const;
/// Print the object in a stream.
virtual std::ostream& print (std::ostream& os) const;
bool isShort_;
private:
typedef Cache < ConstraintSetPtr_t > Constraint_t;
typedef Cache < core::SteeringMethodPtr_t > SteeringMethod_t;
typedef Cache < core::PathValidationPtr_t > PathValidation_t;
/// See pathConstraint member function.
Constraint_t* pathConstraints_;
/// Constraint ensuring that a q_proj will be in to_ and in the
/// same leaf of to_ as the configuration used for initialization.
Constraint_t* configConstraints_;
/// The two ends of the transition.
NodeWkPtr_t from_, to_;
/// True if this path is in node from, False if in node to
NodeWkPtr_t node_;
/// Steering method used to create paths associated to the edge
SteeringMethod_t* steeringMethod_;
/// Path validation associated to the edge
PathValidation_t* pathValidation_;
/// Weak pointer to itself.
EdgeWkPtr_t wkPtr_;
friend class Graph;
}; // class Edge
/// Edge with waypoint.
/// Waypoints are handled recursively, i.e.\ class WaypointEdge contains only a
/// Node and an Edge, the second Edge being itself.
/// In this package, the Node in a WaypointEdge is semantically different from other Node
/// because it does not correspond to a state with different manipulation rules. It has
/// the same rules as another Node (either Edge::from() or Edge::to()).
///
/// Semantically, a waypoint Node is fully part of the WaypointEdge. When a corresponding path
/// reaches it, no planning is required to know what to do next. To the contrary, when a path reaches
/// Edge::from() or Edge::to(), there may be several possibilities.
///
/// \note
/// Implementation details: let's say, between the two nodes \f$N_f\f$ and \f$N_t\f$,
/// two waypoints are required:
/// \f$ N_f \xrightarrow{e_0} n_0 \xrightarrow{e_1} n_1 \xrightarrow{E} N_t\f$.
/// The outmost WaypointEdg contains:
/// \li from: \f$N_f\f$,
/// \li to: \f$N_t\f$,
/// \li constraints: those of edge \f$E\f$,
/// \li waypoint: \f$(E_1, n_1)\f$.
///
/// where \f$E_1\f$ is an instance of class WaypointEdge containing:
/// \li from: \f$N_f\f$,
/// \li to: \f$n_1\f$,
/// \li constraints: those of edge \f$e_1\f$,
/// \li waypoint: \f$(e_0, n_0)\f$.
class HPP_MANIPULATION_DLLAPI WaypointEdge : public Edge
{
public:
/// Create a new WaypointEdge.
static WaypointEdgePtr_t create
(const std::string& name,
const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
virtual bool direction (const core::PathPtr_t& path) const;
virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2) const;
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
/// Return the inner waypoint.
/// \param EdgeType is either Edge or WaypointEdge
template <class EdgeType>
boost::shared_ptr <EdgeType> waypoint (const std::size_t index) const;
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
/// Set the number of waypoints
void nbWaypoints (const size_type number);
std::size_t nbWaypoints () const
{
return waypoints_.size ();
}
/// Set waypoint index with wEdge and wTo.
/// \param wTo is the destination node of wEdge
void setWaypoint (const std::size_t index, const EdgePtr_t wEdge, const NodePtr_t wTo);
/// Get the node in which path after the waypoint is.
NodePtr_t node () const;
protected:
WaypointEdge (const std::string& name) :
Edge (name)
{
}
/// Initialization of the object.
void init (const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
/// Print the object in a stream.
virtual std::ostream& print (std::ostream& os) const;
private:
typedef std::pair < EdgePtr_t, NodePtr_t > Waypoint_t;
typedef std::vector <Waypoint_t> Waypoints_t;
Waypoints_t waypoints_;
mutable matrix_t configs_;
mutable Configuration_t result_;
WaypointEdgeWkPtr_t wkPtr_;
}; // class WaypointEdge
/// Edge that find intersection of level set.
class HPP_MANIPULATION_DLLAPI LevelSetEdge : public Edge
{
public:
virtual ~LevelSetEdge ();
/// Create a new LevelSetEdge.
static LevelSetEdgePtr_t create
(const std::string& name,
const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
virtual bool applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const;
virtual ConstraintSetPtr_t buildConfigConstraint() const;
void buildHistogram ();
LeafHistogramPtr_t histogram () const;
/// \name Foliation definition
/// \{
/// Insert a NumericalConstraint that parametrizes the foliation
void insertParamConstraint (const NumericalConstraintPtr_t& nm,
const SizeIntervals_t& passiveDofs = SizeIntervals_t ());
void insertParamConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
HPP_MANIPULATION_DEPRECATED;
/// Insert a LockedJoint that parametrizes the foliation
void insertParamConstraint (const LockedJointPtr_t lockedJoint);
/// Insert a NumericalConstraint that defines the foliation
///
/// The manifold represented the foliation is defined by this
/// constraints.
void insertConditionConstraint (const NumericalConstraintPtr_t& nm,
const SizeIntervals_t& passiveDofs = SizeIntervals_t ());
/// Insert a LockedJoint that defines the foliation
///
/// The manifold represented the foliation is defined by this
/// constraints.
void insertConditionConstraint (const LockedJointPtr_t lockedJoint);
/// \}
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
protected:
/// Initialization of the object.
void init (const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
LevelSetEdge (const std::string& name);
/// Print the object in a stream.
virtual std::ostream& print (std::ostream& os) const;
/// Populate DrawingAttributes tooltip
virtual void populateTooltip (dot::Tooltip& tp) const;
private:
bool applyConstraintsWithOffset (ConfigurationIn_t qoffset,
ConfigurationIn_t qlevelset, ConfigurationOut_t q) const;
// Parametrizer
// NumericalConstraints_t
NumericalConstraints_t paramNumericalConstraints_;
IntervalsContainer_t paramPassiveDofs_;
// LockedJoints_t
LockedJoints_t paramLockedJoints_;
// Condition
// NumericalConstraints_t
NumericalConstraints_t condNumericalConstraints_;
IntervalsContainer_t condPassiveDofs_;
// LockedJoints_t
LockedJoints_t condLockedJoints_;
/// This histogram will be used to find a good level set.
LeafHistogramPtr_t hist_;
LevelSetEdgeWkPtr_t wkPtr_;
}; // class LevelSetEdge
/// \}
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_EDGE_HH
namespace manipulation {
namespace graph {
/// \addtogroup constraint_graph
/// \{
/// Transition between two states of a constraint graph
///
/// An edge stores two types of constraints.
/// \li <b> Path constraints </b> should be safisfied by paths belonging
/// to the edge. Along any path, the right hand side of the
/// constraint is constant, but can differ between paths. For
/// instance if an edge represents a transit path of a robot
/// that can grasp an object, the right hand side of the
/// constraint represents the position of the object. Along any
/// transit path, the object does not move, but for different paths
/// the object can be at different positions.
/// \sa method pathConstraint.
/// \li <b> Configuration constraints </b> are constraints that
/// configurations in the destination state should satisfy and
/// the constraints that paths should satisfy. For instance, if
/// the edge links a state where the robot does not hold the
/// object to a state where the robot holds the object, the
/// configuration constraints represent a fixed relative
/// position of the object with respect to the gripper and a
/// stable position of the object. Configuration constraints are
/// necessary to generate a configuration in the destination
/// state of the edge that is reachable from a given
/// configuration in the start state by an admissible path.
class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent {
friend class WaypointEdge;
public:
typedef core::RelativeMotion RelativeMotion;
/// Destructor
virtual ~Edge();
/// Create a new empty Edge.
static EdgePtr_t create(const std::string& name, const GraphWkPtr_t& graph,
const StateWkPtr_t& from, const StateWkPtr_t& to);
/// Generate a reachable configuration in the target state
///
/// \param nStart node containing the configuration defining the right
/// hand side of the edge path constraint,
/// \param[in,out] q input configuration used to initialize the
/// numerical solver and output configuration lying
/// in the target state and reachable along the edge
/// from nStart.
virtual bool generateTargetConfig(core::NodePtr_t nStart,
ConfigurationOut_t q) const;
/// Generate a reachable configuration in the target state
///
/// \param qStart node containing the configuration defining the right
/// hand side of the edge path constraint,
/// \param[in,out] q input configuration used to initialize the
/// numerical solver and output configuration lying
/// in the target state and reachable along the edge
/// from nnear.
virtual bool generateTargetConfig(ConfigurationIn_t qStart,
ConfigurationOut_t q) const;
virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
ConfigurationIn_t q2) const;
/// Get the destination
StatePtr_t stateTo() const;
/// Get the origin
StatePtr_t stateFrom() const;
/// Get the state in which path is.
StatePtr_t state() const { return state_.lock(); }
void state(StatePtr_t state) { state_ = state; }
/// Get steering method associated to the edge.
const core::SteeringMethodPtr_t& steeringMethod() const {
return steeringMethod_;
}
/// Get path validation associated to the edge.
const core::PathValidationPtr_t& pathValidation() const {
return pathValidation_;
}
const RelativeMotion::matrix_type& relativeMotion() const {
return relMotion_;
}
/// Update the relative motion matrix
void relativeMotion(const RelativeMotion::matrix_type& m);
/// Set Security margin for a pair of joints
///
/// \param row index of joint1 + 1 in robot,
/// \param col index of joint2 + 1 in robot,
/// \param margin security margin for collision checking between
/// those joints.
///
/// \note set value to matrix [row, col] and matrix [col, row].
void securityMarginForPair(const size_type& row, const size_type& col,
const value_type& margin);
/// Accessor to the security margin.
const matrix_t& securityMargins() const { return securityMargins_; }
/// Get direction of the path compare to the edge
/// \return true is reverse
virtual bool direction(const core::PathPtr_t& path) const;
/// Populate a ConfigProjector with constraints required to generate
/// a path at the intersection of two edges.
virtual bool intersectionConstraint(const EdgePtr_t& other,
ConfigProjectorPtr_t projector) const;
/// Print the object in a stream.
virtual std::ostream& dotPrint(
std::ostream& os,
dot::DrawingAttributes da = dot::DrawingAttributes()) const;
/// Constraint of the destination state and of the path
ConstraintSetPtr_t targetConstraint() const;
void setShort(bool isShort) { isShort_ = isShort; }
bool isShort() const { return isShort_; }
/// Constraint to project a path.
/// \return The initialized constraint.
ConstraintSetPtr_t pathConstraint() const;
protected:
/// Initialization of the object.
void init(const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
const StateWkPtr_t& from, const StateWkPtr_t& to);
/// Constructor
Edge(const std::string& name);
/// Build path and target state constraint set.
virtual ConstraintSetPtr_t buildTargetConstraint();
/// Build path constraints
virtual ConstraintSetPtr_t buildPathConstraint();
virtual void initialize();
/// Print the object in a stream.
virtual std::ostream& print(std::ostream& os) const;
bool isShort_;
private:
/// See pathConstraint member function.
ConstraintSetPtr_t pathConstraints_;
/// Constraint ensuring that a q_proj will be in to_ and in the
/// same leaf of to_ as the configuration used for initialization.
ConstraintSetPtr_t targetConstraints_;
/// The two ends of the transition.
StateWkPtr_t from_, to_;
/// True if this path is in state from, False if in state to
StateWkPtr_t state_;
/// Steering method used to create paths associated to the edge
core::SteeringMethodPtr_t steeringMethod_;
/// Path validation associated to the edge
mutable RelativeMotion::matrix_type relMotion_;
/// matrix of security margins for collision checking between joints
matrix_t securityMargins_;
core::PathValidationPtr_t pathValidation_;
/// Weak pointer to itself.
EdgeWkPtr_t wkPtr_;
friend class Graph;
}; // class Edge
/// Edge with intermediate waypoint states.
///
/// This class implements a transition from one state to another state
/// with intermediate states in-between. This feature is particularly
/// interesting when manipulating objects. Between a state where a gripper
/// does not grasp an object and a state where the same gripper grasps
/// the object, it is useful to add an intermediate state where the
/// gripper is close to the object.
///
/// Waypoints are handled recursively, i.e.\ class WaypointEdge
/// contains only a State and an Edge, the second Edge being
/// itself. In this package, the State in a WaypointEdge is
/// semantically different from other State because it does not
/// correspond to a state with different manipulation rules. It
/// has the same rules as another State (either Edge::stateFrom() or
/// Edge::stateTo()).
///
/// Semantically, a waypoint State is fully part of the WaypointEdge.
/// When a corresponding path reaches it, no planning is required to know
/// what to do next. To the contrary, when a path reaches
/// Edge::stateFrom() or Edge::stateTo(), there may be several
/// possibilities.
///
/// \note
/// Implementation details: let's say, between the two states \f$N_f\f$
/// and \f$N_t\f$, two waypoints are required:
/// \f$ N_f \xrightarrow{e_0} n_0 \xrightarrow{e_1} n_1
/// \xrightarrow{e_2} N_t\f$.
/// The WaypointEdge contains:
/// \li from: \f$N_f\f$,
/// \li to: \f$N_t\f$,
/// \li states: \f$(n_0, n_1)\f$
/// \li transitions: \f$(e_0, e_1, e_2)\f$
/// \li constraints: any calls to the constraints throw,
class HPP_MANIPULATION_DLLAPI WaypointEdge : public Edge {
public:
/// Create a new WaypointEdge.
static WaypointEdgePtr_t create(const std::string& name,
const GraphWkPtr_t& graph,
const StateWkPtr_t& from,
const StateWkPtr_t& to);
virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
ConfigurationIn_t q2) const;
/// Generate a reachable configuration in the target state
///
/// \param qStart node containing the configuration defining the right
/// hand side of the edge path constraint,
/// \param[in,out] q input configuration used to initialize the
/// numerical solver and output configuration lying
/// in the target state and reachable along the edge
/// from nnear.
virtual bool generateTargetConfig(ConfigurationIn_t qStart,
ConfigurationOut_t q) const;
/// Return the index-th edge.
const EdgePtr_t& waypoint(const std::size_t index) const;
/// Print the object in a stream.
virtual std::ostream& dotPrint(
std::ostream& os,
dot::DrawingAttributes da = dot::DrawingAttributes()) const;
/// Set the number of waypoints
void nbWaypoints(const size_type number);
std::size_t nbWaypoints() const { return edges_.size() - 1; }
/// Set waypoint index with wEdge and wTo.
/// \param wTo is the destination state of wEdge
void setWaypoint(const std::size_t index, const EdgePtr_t wEdge,
const StatePtr_t wTo);
protected:
WaypointEdge(const std::string& name) : Edge(name), lastSucceeded_(false) {}
/// Initialization of the object.
void init(const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
const StateWkPtr_t& from, const StateWkPtr_t& to);
/// Initialize each of the internal edges
virtual void initialize();
/// Print the object in a stream.
virtual std::ostream& print(std::ostream& os) const;
private:
Edges_t edges_;
States_t states_;
mutable matrix_t configs_;
mutable bool lastSucceeded_;
WaypointEdgeWkPtr_t wkPtr_;
}; // class WaypointEdge
/// Edge that handles crossed foliations
///
/// Let us consider the following simple constraint graph
/// corresponding to a robot grasping an object with one gripper.
///
/// \image html constraint-graph.png "Simple constraint graph corresponding to a
/// robot grasping an object."
///
/// In order to disambiguate, we assume here that
/// \li transition <b>Grasp object</b> is in \b Placement state,
/// \li transition <b>Release object</b> is in \b Grasp state.
///
/// If state \b Placement is defined by the object lying on a planar
/// polygonal surface, then
/// \li state \b Placement,
/// \li transition \b Transit, and
/// \li transition <b>Grasp object</b>
///
/// are all constrained in a foliated manifold parameterized by the
/// position of the box on the surface.
///
/// Likewise, if the object is cylindrical the grasp may have a degree
/// of freedom corresponding to the angle around z-axis of the gripper
/// with respect to the object. See classes
/// \link hpp::manipulation::Handle Handle\endlink and
/// \link hpp::pinocchio::Gripper Gripper\endlink for details.
/// In this latter case,
/// \li state \b Grasp,
/// \li transition \b Transfer, and
/// \li transition <b>Release object</b>
///
/// are all constrained in a foliated manifold parameterized by the
/// angle around z-axis of the gripper with respect to the object.
///
/// Let us denote
/// \li \c grasp the numerical constraint defining state \b Grasp,
/// \li \c placement the numerical constraint defining state \b Placement,
/// \li \c grasp_comp the parameterized constraint defining a leaf
/// of \c Transfer (the angle between the gripper and the
/// object),
/// \li \c placement_comp the parameterized constraint defining a leaf
/// of \b Placement (the position of the object on the contact
/// surface).
///
/// As explained in <a
/// href="https://hal.archives-ouvertes.fr/hal-01358767">this
/// paper </a>, we are in the crossed foliation case and manipulation RRT
/// will never be able to connect trees expanding in different leaves of
/// the foliation.
///
/// This class solves this issue in the following way by creating an
/// instance of LevelSetEdge between \b Placement and \b Grasp.
///
/// When extending a configuration \f$\mathbf{q}_{start}\f$ in state
/// \b Placement, this transition will produce a target configuration
/// (method \link LevelSetEdge::generateTargetConfig generateTargetConfig)
/// \endlink as follows.
///
/// \li pick a random configuration \f$\mathbf{q}_rand\f$, in the edge
/// histogram (see method \link LevelSetEdge::histogram histogram\endlink)
/// \li compute right hand side of \c grasp_comp with
/// \f$\mathbf{q}_{rand}\f$,
/// \li compute right hand side of \c placement_comp with
/// \f$\mathbf{q}_{start}\f$,
/// \li solve (\c grasp, \c placement, \c placement_comp, \c grasp_comp)
/// using input configuration \f$\mathbf{q}\f$. Note that the
/// parent method Edge::generateTargetConfig does the same without
/// adding \c grasp_comp.
///
/// The constraints parameterizing the target state foliation
/// (\c graps_comp in our example) are passed to class instances
/// using method \link LevelSetEdge::insertParamConstraint
/// insertParamConstraint\endlink.
class HPP_MANIPULATION_DLLAPI LevelSetEdge : public Edge {
public:
virtual ~LevelSetEdge();
/// Create a new LevelSetEdge.
static LevelSetEdgePtr_t create(const std::string& name,
const GraphWkPtr_t& graph,
const StateWkPtr_t& from,
const StateWkPtr_t& to);
/// Generate a reachable configuration in the target state
///
/// \param nStart node containing the configuration defining the right
/// hand side of the edge path constraint,
/// \param[in,out] q input configuration used to initialize the
/// numerical solver and output configuration lying
/// in the target state and reachable along the edge
/// from nStart.
virtual bool generateTargetConfig(core::NodePtr_t nStart,
ConfigurationOut_t q) const;
/// Generate a reachable configuration in the target state
///
/// \param qStart configuration defining the right hand side of the
/// edge path constraint,
/// \param[in,out] q input configuration used to initialize the
/// numerical solver and output configuration lying
/// in the target state and reachable along the edge
/// from nnear.
virtual bool generateTargetConfig(ConfigurationIn_t qStart,
ConfigurationOut_t q) const;
/// Generate a reachable configuration in leaf of target state
/// \param qStart configuration defining the right hand side of the
/// edge path constraint,
/// \param qLeaf configuration used to set the right hand side of
/// the target state foliation. See method
/// \link LevelSetEdge::insertParamConstraint
/// insertParamConstraint\endlink.
bool generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
ConfigurationIn_t qLeaf,
ConfigurationOut_t q) const;
/// Build path and target state constraints
virtual ConstraintSetPtr_t buildTargetConstraint();
/// Build the histogram
/// \sa LevelSetEdge::histogram.
void buildHistogram();
/// Return pointer on histogram of the edge
///
/// The edge histogram is a container of configurations defined by
/// a set of constraints called the <b>condition constraints</b>
/// that a configuration should satisfy to be inserted in the
/// histogram.
///
/// The histogram is passed to the Roadmap via the graph (method
/// Graph::insertHistogram). The roadmap then populates the histogram
/// with all new configurations satisfying the condition constraints.
///
/// The condition constraints should therefore be the constraints of
/// the target state of the level set edge.
///
/// \sa LevelSetEdge::conditionConstraints
/// LevelSetEdge::insertConditionConstraint
LeafHistogramPtr_t histogram() const;
/// \name Foliation definition
/// \{
/// Insert a constraints parameterizing the target state foliation
/// \param nm the numerical constraint,
void insertParamConstraint(const ImplicitPtr_t& nm);
/// Get constraints parameterizing the target state foliation
const NumericalConstraints_t& paramConstraints() const;
/// Insert a condition constraint
/// \sa LevelSetEdge::histogram
void insertConditionConstraint(const ImplicitPtr_t& nm);
/// Get constraints parameterizing the target state foliation
/// \sa LevelSetEdge::histogram
const NumericalConstraints_t& conditionConstraints() const;
/// \}
/// Print the object in a stream.
virtual std::ostream& dotPrint(
std::ostream& os,
dot::DrawingAttributes da = dot::DrawingAttributes()) const;
protected:
/// Initialization of the object.
void init(const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
const StateWkPtr_t& from, const StateWkPtr_t& to);
LevelSetEdge(const std::string& name);
/// Print the object in a stream.
virtual std::ostream& print(std::ostream& os) const;
/// Populate DrawingAttributes tooltip
virtual void populateTooltip(dot::Tooltip& tp) const;
virtual void initialize();
private:
// Parametrizer
// NumericalConstraints_t
NumericalConstraints_t paramNumericalConstraints_;
// Condition
// NumericalConstraints_t
NumericalConstraints_t condNumericalConstraints_;
/// This histogram will be used to find a good level set.
LeafHistogramPtr_t hist_;
LevelSetEdgeWkPtr_t wkPtr_;
}; // class LevelSetEdge
/// \}
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_EDGE_HH
include/hpp/manipulation/graph/fwd.hh
View file @ 539aab52
// 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.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 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/>.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_FWD_HH
# define HPP_MANIPULATION_GRAPH_FWD_HH
#define HPP_MANIPULATION_GRAPH_FWD_HH
#include <hpp/util/pointer.hh>
#include <hpp/statistics/distribution.hh>
#include <hpp/util/pointer.hh>
#include <vector>
namespace hpp {
namespace manipulation {
namespace graph {
HPP_PREDEF_CLASS (Graph);
HPP_PREDEF_CLASS (Node);
HPP_PREDEF_CLASS (Edge);
HPP_PREDEF_CLASS (WaypointEdge);
HPP_PREDEF_CLASS (LevelSetEdge);
HPP_PREDEF_CLASS (NodeSelector);
HPP_PREDEF_CLASS (GuidedNodeSelector);
HPP_PREDEF_CLASS (GraphComponent);
typedef boost::shared_ptr < Graph > GraphPtr_t;
typedef boost::shared_ptr < Node > NodePtr_t;
typedef boost::shared_ptr < Edge > EdgePtr_t;
typedef boost::shared_ptr < WaypointEdge > WaypointEdgePtr_t;
typedef boost::shared_ptr < LevelSetEdge > LevelSetEdgePtr_t;
typedef boost::shared_ptr < NodeSelector > NodeSelectorPtr_t;
typedef boost::shared_ptr < GuidedNodeSelector > GuidedNodeSelectorPtr_t;
typedef boost::shared_ptr < GraphComponent > GraphComponentPtr_t;
typedef std::vector < NodePtr_t > Nodes_t;
typedef std::vector < EdgePtr_t > Edges_t;
typedef ::hpp::statistics::DiscreteDistribution< EdgePtr_t >::Weight_t Weight_t;
typedef ::hpp::statistics::DiscreteDistribution< EdgePtr_t > Neighbors_t;
typedef std::vector < NodeSelectorPtr_t > NodeSelectors_t;
namespace manipulation {
namespace graph {
HPP_PREDEF_CLASS(Graph);
HPP_PREDEF_CLASS(Edge);
HPP_PREDEF_CLASS(State);
HPP_PREDEF_CLASS(WaypointEdge);
HPP_PREDEF_CLASS(LevelSetEdge);
HPP_PREDEF_CLASS(StateSelector);
HPP_PREDEF_CLASS(GraphComponent);
HPP_PREDEF_CLASS(GuidedStateSelector);
typedef shared_ptr<Graph> GraphPtr_t;
typedef shared_ptr<State> StatePtr_t;
typedef shared_ptr<Edge> EdgePtr_t;
typedef shared_ptr<WaypointEdge> WaypointEdgePtr_t;
typedef shared_ptr<LevelSetEdge> LevelSetEdgePtr_t;
typedef shared_ptr<StateSelector> StateSelectorPtr_t;
typedef shared_ptr<GuidedStateSelector> GuidedStateSelectorPtr_t;
typedef shared_ptr<GraphComponent> GraphComponentPtr_t;
typedef std::vector<GraphComponentWkPtr_t> GraphComponents_t;
typedef std::vector<StatePtr_t> States_t;
typedef std::vector<EdgePtr_t> Edges_t;
typedef ::hpp::statistics::DiscreteDistribution<EdgePtr_t>::Weight_t Weight_t;
typedef ::hpp::statistics::DiscreteDistribution<EdgePtr_t> Neighbors_t;
typedef std::vector<StateSelectorPtr_t> StateSelectors_t;
typedef hpp::core::segments_t segments_t;
typedef std::vector<segments_t> IntervalsContainer_t;
typedef hpp::core::NumericalConstraints_t NumericalConstraints_t;
typedef hpp::core::LockedJoints_t LockedJoints_t;
typedef hpp::core::Constraint Constraint;
typedef hpp::core::ConstraintPtr_t ConstraintPtr_t;
typedef hpp::core::LockedJoint LockedJoint;
typedef hpp::core::LockedJointPtr_t LockedJointPtr_t;
typedef hpp::core::ConfigProjector ConfigProjector;
typedef hpp::core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
typedef hpp::core::Equality Equality;
typedef hpp::core::ComparisonTypePtr_t ComparisonTypePtr_t;
typedef hpp::core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
typedef hpp::core::SizeIntervals_t SizeIntervals_t;
typedef std::vector <SizeIntervals_t> IntervalsContainer_t;
typedef hpp::core::NumericalConstraints_t NumericalConstraints_t;
typedef hpp::core::LockedJoints_t LockedJoints_t;
class Histogram;
class StateHistogram;
class LeafHistogram;
typedef shared_ptr<Histogram> HistogramPtr_t;
typedef shared_ptr<StateHistogram> StateHistogramPtr_t;
typedef shared_ptr<LeafHistogram> LeafHistogramPtr_t;
typedef std::list<HistogramPtr_t> Histograms_t;
class Histogram;
class NodeHistogram;
class LeafHistogram;
typedef boost::shared_ptr <Histogram> HistogramPtr_t;
typedef boost::shared_ptr <NodeHistogram> NodeHistogramPtr_t;
typedef boost::shared_ptr <LeafHistogram> LeafHistogramPtr_t;
} // namespace graph
} // namespace manipulation
} // namespace hpp
class Validation;
typedef shared_ptr<Validation> ValidationPtr_t;
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_FWD_HH
#endif // HPP_MANIPULATION_GRAPH_FWD_HH
include/hpp/manipulation/graph/graph-component.hh
View file @ 539aab52
// 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.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 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/>.
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH
# define HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH
#define HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH
# include <string>
# include <ostream>
# include <hpp/util/exception.hh>
#include <ostream>
#include <string>
# include "hpp/manipulation/config.hh"
# include "hpp/manipulation/deprecated.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
# include "hpp/manipulation/graph/dot.hh"
#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/deprecated.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/dot.hh"
#include "hpp/manipulation/graph/fwd.hh"
namespace hpp {
namespace manipulation {
namespace graph {
HPP_MAKE_EXCEPTION ( HPP_MANIPULATION_DLLAPI, Bad_function_call );
/// \defgroup constraint_graph Constraint Graph
/// \addtogroup constraint_graph
/// \{
/// Define common methods of the graph components.
class HPP_MANIPULATION_DLLAPI GraphComponent
{
public:
/// Get the component by its ID. The validity of the GraphComponent
/// is not checked.
static GraphComponentWkPtr_t get(std::size_t id);
/// The list of elements
static const std::vector < GraphComponentWkPtr_t >& components ();
/// Get the component name.
const std::string& name() const;
/// Set the component name.
/// \deprecated The name given at construction cannot be modified.
void name(const std::string& name) HPP_MANIPULATION_DEPRECATED;
/// Return the component id.
const std::size_t& id () const
{
return id_;
}
/// Add core::NumericalConstraint to the component.
/// \param passiveDofs see ConfigProjector::addNumericalConstraint
// for more information.
virtual void addNumericalConstraint (
const NumericalConstraintPtr_t& numConstraint,
const SizeIntervals_t& passiveDofs = SizeIntervals_t ());
/// Add core::DifferentiableFunction to the component.
virtual void addNumericalConstraint
(const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
HPP_MANIPULATION_DEPRECATED;
/// Add core::LockedJoint constraint to the component.
virtual void addLockedJointConstraint
(const LockedJointPtr_t& constraint);
/// Insert the numerical constraints in a ConfigProjector
/// \return true is at least one NumericalConstraintPtr_t was inserted.
bool insertNumericalConstraints (ConfigProjectorPtr_t& proj) const;
/// Insert the LockedJoint constraints in a ConstraintSet
/// \return true is at least one LockedJointPtr_t was inserted.
bool insertLockedJoints (ConfigProjectorPtr_t& cs) const;
/// Get a reference to the NumericalConstraints_t
const NumericalConstraints_t& numericalConstraints() const;
/// Get a reference to the NumericalConstraints_t
const IntervalsContainer_t& passiveDofs() const;
/// Get a reference to the LockedJoints_t
const LockedJoints_t& lockedJoints () const;
/// Set the parent graph.
void parentGraph(const GraphWkPtr_t& parent);
/// Set the parent graph.
GraphPtr_t parentGraph() const;
/// Print the component in DOT language.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
protected:
/// Initialize the component
void init (const GraphComponentWkPtr_t& weak);
GraphComponent(const std::string& name) : name_ (name), id_(-1)
{}
/// Stores the numerical constraints.
NumericalConstraints_t numericalConstraints_;
/// Stores the passive dofs for each numerical constraints.
IntervalsContainer_t passiveDofs_;
/// List of LockedJoint constraints
LockedJoints_t lockedJoints_;
/// A weak pointer to the parent graph.
GraphWkPtr_t graph_;
/// Print the object in a stream.
virtual std::ostream& print (std::ostream& os) const;
friend std::ostream& operator<< (std::ostream&, const GraphComponent&);
/// Populate DrawingAttributes tooltip
virtual void populateTooltip (dot::Tooltip& tp) const;
private:
/// Keep track of the created components in order to retrieve them
/// easily.
static std::vector < GraphComponentWkPtr_t > components_;
namespace manipulation {
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
namespace graph {
/// \defgroup constraint_graph Constraint Graph
/// \addtogroup constraint_graph
/// \{
/// Define common methods of the graph components.
class HPP_MANIPULATION_DLLAPI GraphComponent {
public:
virtual ~GraphComponent() {};
/// Get the component name.
const std::string& name() const;
/// Return the component id.
const std::size_t& id() const { return id_; }
/// Add constraint to the component.
virtual void addNumericalConstraint(const ImplicitPtr_t& numConstraint);
/// Add a cost function Implicit to the component.
virtual void addNumericalCost(const ImplicitPtr_t& numCost);
/// Reset the numerical constraints stored in the component.
virtual void resetNumericalConstraints();
/// Insert the numerical constraints in a ConfigProjector
/// \return true is at least one ImplicitPtr_t was inserted.
bool insertNumericalConstraints(ConfigProjectorPtr_t& proj) const;
/// Get a reference to the NumericalConstraints_t
const NumericalConstraints_t& numericalConstraints() const;
/// Get a reference to the NumericalConstraints_t
const NumericalConstraints_t& numericalCosts() const;
/// Set the parent graph.
void parentGraph(const GraphWkPtr_t& parent);
/// Set the parent graph.
GraphPtr_t parentGraph() const;
/// Print the component in DOT language.
virtual std::ostream& dotPrint(
std::ostream& os,
dot::DrawingAttributes da = dot::DrawingAttributes()) const;
/// Invalidate the component
/// The component needs to be initialized again.
virtual void invalidate() { isInit_ = false; }
/// Set whether hierachical constraints are solved level by level
/// \sa hpp::constraints::solver::HierarchicalIterative
void solveLevelByLevel(bool solveLevelByLevel) {
solveLevelByLevel_ = solveLevelByLevel;
}
/// Get whether hierachical constraints are solved level by level
/// \sa hpp::constraints::solver::HierarchicalIterative
bool solveLevelByLevel() const { return solveLevelByLevel_; }
protected:
/// Initialize the component
void init(const GraphComponentWkPtr_t& weak);
GraphComponent(const std::string& name)
: isInit_(false), name_(name), id_(-1), solveLevelByLevel_(false) {}
/// Stores the numerical constraints.
NumericalConstraints_t numericalConstraints_;
/// Stores the numerical costs.
NumericalConstraints_t numericalCosts_;
/// A weak pointer to the parent graph.
GraphWkPtr_t graph_;
bool isInit_;
void throwIfNotInitialized() const;
/// Print the object in a stream.
virtual std::ostream& print(std::ostream& os) const;
friend std::ostream& operator<<(std::ostream&, const GraphComponent&);
/// Populate DrawingAttributes tooltip
virtual void populateTooltip(dot::Tooltip& tp) const;
virtual void initialize() = 0;
private:
/// Name of the component.
std::string name_;
/// Weak pointer to itself.
GraphComponentWkPtr_t wkPtr_;
/// ID of the component (index in components vector).
std::size_t id_;
/// Whether the constraints are solved level by level
bool solveLevelByLevel_;
friend class Graph;
};
std::ostream& operator<<(std::ostream& os, const GraphComponent& graphComp);
/// Name of the component.
std::string name_;
/// Weak pointer to itself.
GraphComponentWkPtr_t wkPtr_;
/// ID of the component (index in components vector).
std::size_t id_;
};
std::ostream& operator<< (std::ostream& os, const GraphComponent& graphComp);
/// \}
} // namespace graph
} // namespace manipulation
/// \}
} // namespace graph
} // namespace manipulation
} // namespace hpp
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH
#endif // HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH
include/hpp/manipulation/graph/graph.hh
View file @ 539aab52
// 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.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 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/>.
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_GRAPH_HH
# define HPP_MANIPULATION_GRAPH_GRAPH_HH
#define HPP_MANIPULATION_GRAPH_GRAPH_HH
#include <tuple>
# include "hpp/manipulation/config.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
# include "hpp/manipulation/graph/graph-component.hh"
#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/constraint-set.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/fwd.hh"
#include "hpp/manipulation/graph/graph-component.hh"
namespace hpp {
namespace manipulation {
namespace graph {
/// \addtogroup constraint_graph
/// \{
/// Description of the constraint graph.
///
/// This class contains a graph representing a robot with several
/// end-effectors.
///
/// One must make sure not to create loop with shared pointers.
/// To ensure that, the classes are defined as follow:
/// - A Graph owns (i.e. has a shared pointer to) the NodeSelector s
/// - A NodeSelector owns the Node s related to one gripper.
/// - A Node owns its outgoing Edge s.
/// - An Edge does not own anything.
class HPP_MANIPULATION_DLLAPI Graph : public GraphComponent
{
public:
/// Create a new Graph.
///
/// \param robot a manipulation robot
/// \param problem a pointer to the problem
static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
const ProblemPtr_t& problem);
/// Create and insert a NodeSelector inside the graph.
NodeSelectorPtr_t createNodeSelector (const std::string& name);
/// Set the nodeSelector
/// \warning This should be done before adding nodes to the node
/// selector otherwise the pointer to the parent graph will NOT be
/// valid.
void nodeSelector (NodeSelectorPtr_t ns);
/// Get the nodeSelector
NodeSelectorPtr_t nodeSelector () const
{
return nodeSelector_;
}
/// Returns the states of a configuration.
NodePtr_t getNode (ConfigurationIn_t config) const;
/// Returns the state of a roadmap node
NodePtr_t getNode(RoadmapNodePtr_t node) const;
/// Get possible edges between two nodes.
Edges_t getEdges (const NodePtr_t& from, const NodePtr_t& to) const;
/// Select randomly outgoing edge of the given node.
EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
/// Constraint to project onto the Node.
/// \param the Node_t on which to project.
/// \return The initialized projector.
ConstraintSetPtr_t configConstraint (const NodePtr_t& node);
/// Constraint to project onto the same leaf as config.
/// \param edges a list of edges defining the foliation.
/// \return The constraint.
ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge);
/// Get error of a config with respect to a node constraint
///
/// \param config Configuration,
/// \param node node containing the constraint to check config against
/// \retval error the error of the node constraint for the
/// configuration
/// \return whether the configuration belongs to the node.
/// Call method core::ConstraintSet::isSatisfied for the node
/// constraints.
bool getConfigErrorForNode (ConfigurationIn_t config,
const NodePtr_t& node, vector_t& error);
/// Constraint to project a path.
/// \param edge a list of edges defining the foliation.
/// \return The constraint.
ConstraintSetPtr_t pathConstraint (const EdgePtr_t& edge);
/// Set maximal number of iterations
void maxIterations (size_type iterations);
/// Get maximal number of iterations in config projector
size_type maxIterations () const;
/// Set error threshold
void errorThreshold (const value_type& threshold);
/// Get error threshold in config projector
value_type errorThreshold () const;
/// Get the robot.
const DevicePtr_t& robot () const;
/// Get the steering Method
const ProblemPtr_t& problem () const;
/// Register an histogram representing a foliation
void insertHistogram (const graph::HistogramPtr_t& hist)
{
hists_.push_back (hist);
}
/// Get the histograms
std::list<HistogramPtr_t>& histograms ()
{
return hists_;
}
/// Print the component in DOT language.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
protected:
/// Initialization of the object.
void init (const GraphWkPtr_t& weak, DevicePtr_t robot);
/// Constructor
/// \param sm a steering method to create paths from edges
Graph (const std::string& name, const ProblemPtr_t& problem) :
GraphComponent (name), problem_ (problem)
{}
/// Print the object in a stream.
std::ostream& print (std::ostream& os) const;
private:
/// This list contains a node selector for each end-effector.
NodeSelectorPtr_t nodeSelector_;
/// A set of constraints that will always be used, for example
/// stability constraints.
ConstraintPtr_t constraints_;
/// Keep a pointer to the composite robot.
DevicePtr_t robot_;
/// Weak pointer to itself.
GraphWkPtr_t wkPtr_;
/// Map of constraint sets (from Node).
typedef std::map < NodePtr_t, ConstraintSetPtr_t > MapFromNode;
typedef std::pair < NodePtr_t, ConstraintSetPtr_t > PairNodeConstraints;
MapFromNode constraintSetMapFromNode_;
/// List of histograms
std::list<HistogramPtr_t> hists_;
/// Map of constraint sets (from Edge).
typedef std::map < EdgePtr_t, ConstraintSetPtr_t > MapFromEdge;
typedef std::pair < EdgePtr_t, ConstraintSetPtr_t > PairEdgeConstraints;
MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
ProblemPtr_t problem_;
value_type errorThreshold_;
size_type maxIterations_;
}; // Class Graph
/// \}
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_GRAPH_HH
namespace manipulation {
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
namespace graph {
/// \addtogroup constraint_graph
/// \{
/// Description of the constraint graph.
///
/// This class contains a graph representing a a manipulation problem
///
/// One must make sure not to create loop with shared pointers.
/// To ensure that, the classes are defined as follow:
/// - A Graph owns (i.e. has a shared pointer to) the StateSelector s
/// - A StateSelector owns the Node s related to one gripper.
/// - A State owns its outgoing Edge s.
/// - An Edge does not own anything.
///
/// \note The graph and all its components have a unique index starting
/// at 0 for the graph itself. The index of a component can be retrieved
/// using method GraphComponent::id.
class HPP_MANIPULATION_DLLAPI Graph : public GraphComponent {
public:
/// Create a new Graph.
///
/// \param robot a manipulation robot
/// \param problem a pointer to the problem
static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
const ProblemPtr_t& problem);
GraphPtr_t self() const { return wkPtr_.lock(); }
/// Create and insert a state selector inside the graph.
StateSelectorPtr_t createStateSelector(const std::string& name);
/// Set the state selector
/// \warning This should be done before adding nodes to the node
/// selector otherwise the pointer to the parent graph will NOT be
/// valid.
void stateSelector(StateSelectorPtr_t ns);
/// Get the state selector
StateSelectorPtr_t stateSelector() const { return stateSelector_; }
/// Returns the state of a configuration.
StatePtr_t getState(ConfigurationIn_t config) const;
/// Returns the state of a roadmap node
StatePtr_t getState(RoadmapNodePtr_t node) const;
/// Get possible edges between two nodes.
Edges_t getEdges(const StatePtr_t& from, const StatePtr_t& to) const;
/// Select randomly outgoing edge of the given node.
EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
/// Clear the vector of constraints and complements
/// \sa registerConstraints
void clearConstraintsAndComplement();
/// Register a triple of constraints to be inserted in nodes and edges
/// \param constraint a constraint (grasp of placement)
/// \param complement the complement constraint
/// \param both combination of the constraint and its complement. Both
/// constraints together corresponds to a full relative
/// transformation constraint
/// When inserting constraints in transitions of the graph,
/// in many cases, a constraint is associated to a state and
/// the complement constraint is associated to the
/// transition itself. Registering those constraints
/// priorly to graph construction makes possible to replace
/// the constraint and its complement by the combination of
/// both that is an explicit constraint.
void registerConstraints(const ImplicitPtr_t& constraint,
const ImplicitPtr_t& complement,
const ImplicitPtr_t& both);
/// Test whether two constraints are complement of one another
///
/// \param constraint, complement two constraints to test
/// \retval combinationOfBoth constraint corresponding to combining
/// constraint and complement if result is true,
/// unchanged otherwise.
/// \return whether complement is the complement of constraint.
/// Two constraints are complement of one another if and only if
/// combined they constitute a complement relative transformation
/// constraint. \sa Graph::registerConstraints
/// \warning argument order matters.
bool isComplement(const ImplicitPtr_t& constraint,
const ImplicitPtr_t& complement,
ImplicitPtr_t& combinationOfBoth) const;
/// Return the vector of tuples as registered in registerConstraints
/// \return a vector of tuples (c, c/complement, c/both) each of them
/// corresponding to a constraint, the complement constraint
/// and the combination of boths.
const ConstraintsAndComplements_t& constraintsAndComplements() const;
/// Constraint to project onto the Node.
/// \param state the state on which to project.
/// \return The initialized projector.
ConstraintSetPtr_t configConstraint(const StatePtr_t& state) const;
/// Constraints a configuration in target state should satisfy
/// \param edge a transition
/// \return The set of constraints a configuration lying in the
/// target state of the edge should satisfy. This set
/// includes the paths constraints of the edge.
/// \sa Edge::targetConstraint.
ConstraintSetPtr_t targetConstraint(const EdgePtr_t& edge) const;
/// Get error of a config with respect to a state constraint
///
/// \param config Configuration,
/// \param state state containing the constraint to check config against
/// \retval error the error of the state constraint for the
/// configuration
/// \return whether the configuration belongs to the state.
/// Call method core::ConstraintSet::isSatisfied for the state
/// constraints.
bool getConfigErrorForState(ConfigurationIn_t config, const StatePtr_t& state,
vector_t& error) const;
/// Get error of a config with respect to an edge constraint
///
/// \param config Configuration,
/// \param edge edge containing the constraint to check config against
/// \retval error the error of the edge constraint for the
/// configuration
/// \return whether the configuration can be a start point of a path
// of the edge
/// Call core::ConfigProjector::rightHandSideFromConfig with
/// input configuration and method core::ConstraintSet::isSatisfied
/// for the edge constraint.
bool getConfigErrorForEdge(ConfigurationIn_t config, const EdgePtr_t& edge,
vector_t& error) const;
/// Get error of a config with respect to an edge foliation leaf
///
/// \param leafConfig Configuration that determines the foliation leaf
/// \param config Configuration the error of which is computed
/// \retval error the error
/// \return whether config can be the end point of a path of the edge
/// starting at leafConfig
/// Call methods core::ConfigProjector::rightHandSideFromConfig with
/// leafConfig and then core::ConstraintSet::isSatisfied with config.
/// on the edge constraints.
bool getConfigErrorForEdgeLeaf(ConfigurationIn_t leafConfig,
ConfigurationIn_t config,
const EdgePtr_t& edge, vector_t& error) const;
/// Get error of a config with respect to the target of an edge foliation leaf
///
/// \param leafConfig Configuration that determines the foliation leaf
/// \param config Configuration the error of which is computed
/// \retval error the error
/// \return whether config can be the end point of a path of the edge
/// starting at leafConfig
/// Call methods core::ConfigProjector::rightHandSideFromConfig with
/// leafConfig and then core::ConstraintSet::isSatisfied with config.
/// on the edge constraints.
bool getConfigErrorForEdgeTarget(ConfigurationIn_t leafConfig,
ConfigurationIn_t config,
const EdgePtr_t& edge,
vector_t& error) const;
/// Constraint to project a path.
/// \param edge a list of edges defining the foliation.
/// \return The constraint.
ConstraintSetPtr_t pathConstraint(const EdgePtr_t& edge) const;
/// Set maximal number of iterations
void maxIterations(size_type iterations);
/// Get maximal number of iterations in config projector
size_type maxIterations() const;
/// Set error threshold
void errorThreshold(const value_type& threshold);
/// Get error threshold in config projector
value_type errorThreshold() const;
/// Get the robot.
const DevicePtr_t& robot() const;
/// Get the problem
const ProblemPtr_t& problem() const;
/// Set the problem
void problem(const ProblemPtr_t& problem);
/// Register an histogram representing a foliation
void insertHistogram(const graph::HistogramPtr_t& hist) {
hists_.push_back(hist);
}
/// Get the histograms
const Histograms_t& histograms() const { return hists_; }
/// Get the component by its ID.
GraphComponentWkPtr_t get(std::size_t id) const;
std::size_t nbComponents() const { return components_.size(); }
/// Print the component in DOT language.
virtual std::ostream& dotPrint(
std::ostream& os,
dot::DrawingAttributes da = dot::DrawingAttributes()) const;
/// Initialize all components of the graph (edges and states)
virtual void initialize();
/// Invalidate all states and edges of the graph
virtual void invalidate();
protected:
/// Initialization of the object.
void init(const GraphWkPtr_t& weak, DevicePtr_t robot);
/// Constructor
/// \param problem a pointer to the problem
Graph(const std::string& name, const ProblemPtr_t& problem);
/// Print the object in a stream.
std::ostream& print(std::ostream& os) const;
private:
/// The list of elements
GraphComponents_t& components();
/// Keep track of the created components
GraphComponents_t components_;
/// This list contains a state selector for each end-effector.
StateSelectorPtr_t stateSelector_;
/// A set of constraints that will always be used, for example
/// stability constraints.
ConstraintPtr_t constraints_;
/// Keep a pointer to the composite robot.
DevicePtr_t robot_;
/// Weak pointer to itself.
GraphWkPtr_t wkPtr_;
/// Map of constraint sets (from State).
typedef std::map<StatePtr_t, ConstraintSetPtr_t> MapFromState;
typedef std::pair<StatePtr_t, ConstraintSetPtr_t> PairStateConstraints;
MapFromState constraintSetMapFromState_;
/// List of histograms
Histograms_t hists_;
/// Map of constraint sets (from Edge).
typedef std::map<EdgePtr_t, ConstraintSetPtr_t> MapFromEdge;
typedef std::pair<EdgePtr_t, ConstraintSetPtr_t> PairEdgeConstraints;
MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
ProblemPtr_t problem_;
value_type errorThreshold_;
size_type maxIterations_;
ConstraintsAndComplements_t constraintsAndComplements_;
friend class GraphComponent;
}; // Class Graph
/// \}
} // namespace graph
} // namespace manipulation
} // namespace hpp
BOOST_CLASS_EXPORT_KEY(hpp::manipulation::graph::Graph)
#endif // HPP_MANIPULATION_GRAPH_GRAPH_HH
include/hpp/manipulation/graph/guided-node-selector.hh deleted 100644 → 0
View file @ 0e448947
// 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/>.
#ifndef HPP_MANIPULATION_GRAPH_GUIDED_NODE_SELECTOR_HH
# define HPP_MANIPULATION_GRAPH_GUIDED_NODE_SELECTOR_HH
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/fwd.hh"
#include "hpp/manipulation/graph/node-selector.hh"
namespace hpp {
namespace manipulation {
namespace graph {
class HPP_MANIPULATION_DLLAPI GuidedNodeSelector : public NodeSelector
{
public:
/// Create a new GuidedNodeSelector.
static GuidedNodeSelectorPtr_t create(const std::string& name,
const core::RoadmapPtr_t& roadmap);
/// Set the target
void setNodeList (const Nodes_t& nodeList);
/// Select randomly an outgoing edge of the given node.
virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
/// Print the object in a stream.
std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
protected:
/// Initialization of the object.
void init (const GuidedNodeSelectorPtr_t& weak);
/// Constructor
GuidedNodeSelector (const std::string& name,
const core::RoadmapPtr_t roadmap) :
NodeSelector (name), roadmap_ (roadmap)
{}
/// Print the object in a stream.
std::ostream& print (std::ostream& os) const;
private:
/// The target
Nodes_t nodeList_;
/// The roadmap
core::RoadmapPtr_t roadmap_;
/// Weak pointer to itself.
GuidedNodeSelectorWkPtr_t wkPtr_;
}; // Class NodeSelector
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_GUIDED_NODE_SELECTOR_HH
include/hpp/manipulation/graph/guided-state-selector.hh 0 → 100644
View file @ 539aab52
// Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_GUIDED_STATE_SELECTOR_HH
#define HPP_MANIPULATION_GRAPH_GUIDED_STATE_SELECTOR_HH
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/fwd.hh"
#include "hpp/manipulation/graph/state-selector.hh"
namespace hpp {
namespace manipulation {
namespace graph {
class HPP_MANIPULATION_DLLAPI GuidedStateSelector : public StateSelector {
public:
/// Create a new GuidedStateSelector.
static GuidedStateSelectorPtr_t create(const std::string& name,
const core::RoadmapPtr_t& roadmap);
/// Set the target
void setStateList(const States_t& stateList);
/// Select randomly an outgoing edge of the given node.
virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
/// Print the object in a stream.
std::ostream& dotPrint(std::ostream& os, dot::DrawingAttributes da =
dot::DrawingAttributes()) const;
protected:
/// Initialization of the object.
void init(const GuidedStateSelectorPtr_t& weak);
/// Constructor
GuidedStateSelector(const std::string& name, const core::RoadmapPtr_t roadmap)
: StateSelector(name), roadmap_(roadmap) {}
/// Print the object in a stream.
std::ostream& print(std::ostream& os) const;
private:
/// The target
States_t stateList_;
/// The roadmap
core::RoadmapPtr_t roadmap_;
/// Weak pointer to itself.
GuidedStateSelectorWkPtr_t wkPtr_;
}; // Class StateSelector
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_GUIDED_STATE_SELECTOR_HH
include/hpp/manipulation/graph/helper.hh
View file @ 539aab52
// Copyright (c) 2016, 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.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 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/>.
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_HELPER_HH
# define HPP_MANIPULATION_GRAPH_HELPER_HH
# include <string>
# include <algorithm>
#define HPP_MANIPULATION_GRAPH_HELPER_HH
# include <boost/tuple/tuple.hpp>
#include <algorithm>
#include <string>
#include <tuple>
# include "hpp/manipulation/config.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/fwd.hh"
namespace hpp {
namespace manipulation {
namespace graph {
namespace helper {
/// \defgroup helper Helpers to build the graph of constraints
/// \addtogroup helper
/// \{
struct NumericalConstraintsAndPassiveDofs {
NumericalConstraints_t nc;
IntervalsContainer_t pdof;
NumericalConstraintsAndPassiveDofs merge
(const NumericalConstraintsAndPassiveDofs& other) {
NumericalConstraintsAndPassiveDofs ret;
// ret.nc.reserve (nc.size() + other.nc.size());
ret.pdof.reserve (pdof.size() + other.pdof.size());
std::copy (nc.begin(), nc.end(), ret.nc.begin());
std::copy (other.nc.begin(), other.nc.end(), ret.nc.begin());
std::copy (pdof.begin(), pdof.end(), ret.pdof.begin());
std::copy (other.pdof.begin(), other.pdof.end(), ret.pdof.begin());
return ret;
}
template <bool forPath> void addToComp (GraphComponentPtr_t comp) const;
template <bool param> void specifyFoliation (LevelSetEdgePtr_t lse) const;
};
struct FoliatedManifold {
// Manifold definition
NumericalConstraintsAndPassiveDofs nc;
LockedJoints_t lj;
NumericalConstraintsAndPassiveDofs nc_path;
// Foliation definition
NumericalConstraintsAndPassiveDofs nc_fol;
LockedJoints_t lj_fol;
FoliatedManifold merge (const FoliatedManifold& other) {
FoliatedManifold both;
both.nc = nc.merge (other.nc);
both.nc_path = nc_path.merge (other.nc_path);
std::copy (lj.begin (), lj.end (), both.lj.end ());
std::copy (other.lj.begin (), other.lj.end (), both.lj.end ());
return both;
}
void addToNode (NodePtr_t comp) const;
void addToEdge (EdgePtr_t comp) const;
void specifyFoliation (LevelSetEdgePtr_t lse) const;
bool foliated () const {
return !lj_fol.empty () || !nc_fol.nc.empty ();
}
bool empty () const {
return lj.empty () && nc.nc.empty ();
}
};
enum GraspingCase {
NoGrasp = 0,
GraspOnly = 1 << 0,
WithPreGrasp = 1 << 1
};
enum PlacementCase {
NoPlace = 1 << 2,
PlaceOnly = 1 << 3,
WithPrePlace = 1 << 4
};
/// Create edges according to the case.
/// gCase is a logical OR combination of GraspingCase and PlacementCase
///
/// When an argument is not relevant, use the default constructor
/// of FoliatedManifold
template < int gCase >
Edges_t createEdges (
const std::string& forwName, const std::string& backName,
const NodePtr_t& from, const NodePtr_t& to,
const size_type& wForw, const size_type& wBack,
const FoliatedManifold& grasp, const FoliatedManifold& pregrasp,
const FoliatedManifold& place, const FoliatedManifold& preplace,
const bool levelSetGrasp, const bool levelSetPlace,
const FoliatedManifold& submanifoldDef = FoliatedManifold ()
);
EdgePtr_t createLoopEdge (
const std::string& loopName,
const NodePtr_t& node,
const size_type& w,
const bool levelSet,
const FoliatedManifold& submanifoldDef = FoliatedManifold ()
);
/// Create a waypoint edge taking into account:
/// \li grasp
/// \li placement
/// \li preplacement
/// Create a waypoint edge taking into account
/// \li grasp
/// \li pregrasp
/// \li placement
/// \todo when the handle is a free flying object, add the robot DOFs
/// as passive dofs to the numerical constraints for paths
void graspManifold (
const GripperPtr_t& gripper, const HandlePtr_t& handle,
FoliatedManifold& grasp, FoliatedManifold& pregrasp);
/// The placement foliation constraint is built using
/// hpp::constraints::ConvexShapeMatcherComplement
void strictPlacementManifold (
const NumericalConstraintPtr_t placement,
const NumericalConstraintPtr_t preplacement,
const NumericalConstraintPtr_t placementComplement,
FoliatedManifold& place, FoliatedManifold& preplace);
/// The placement foliation constraint is built locked joints
/// It is faster than strictPlacementManifold but the foliation
/// parametrisation is redundant.
void relaxedPlacementManifold (
const NumericalConstraintPtr_t placement,
const NumericalConstraintPtr_t preplacement,
const LockedJoints_t objectLocks,
FoliatedManifold& place, FoliatedManifold& preplace);
typedef boost::tuple <NumericalConstraintPtr_t,
NumericalConstraintPtr_t,
LockedJoints_t>
PlacementConstraint_t;
typedef std::vector <HandlePtr_t> Handles_t;
typedef std::vector <GripperPtr_t> Grippers_t;
/// Tuple representing an object as follows:
/// \li PlacementConstraint_t constraint to place the object
/// \li Handles_t list of handles of the object
/// \li std::size_t the index of this tuple in Objects_t.
/// \note the index must be unique, as object equallity is checked using this index.
typedef boost::tuple <PlacementConstraint_t, Handles_t, std::size_t> Object_t;
typedef std::vector <Object_t> Objects_t;
/// Fill a Graph
///
/// \note It is assumed that a gripper can grasp only one handle and each
/// handle cannot be grasped by several grippers at the same time.
///
/// \param[in,out] graph must be an initialized empty Graph.
void graphBuilder (
const Objects_t& objects,
const Grippers_t& grippers,
GraphPtr_t graph);
struct ObjectDef_t {
std::string name;
StringList_t handles, shapes;
};
GraphPtr_t graphBuilder (
const ProblemSolverPtr_t& ps,
const std::string& graphName,
const StringList_t& griNames,
const std::list <ObjectDef_t>& objs,
const StringList_t& envNames,
const value_type& prePlaceWidth = 0.05);
/// \}
} // namespace helper
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_HELPER_HH
namespace manipulation {
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
namespace graph {
namespace helper {
/// \defgroup helper Helpers to build the graph of constraints
/// \addtogroup helper
/// \{
NumericalConstraints_t merge_nc(const NumericalConstraints_t& a,
const NumericalConstraints_t& b) {
NumericalConstraints_t nc;
nc.reserve(a.size() + b.size());
std::copy(a.begin(), a.end(), nc.begin());
std::copy(b.begin(), b.end(), nc.begin());
return nc;
}
struct FoliatedManifold {
// Manifold definition
NumericalConstraints_t nc;
LockedJoints_t lj;
NumericalConstraints_t nc_path;
// Foliation definition
NumericalConstraints_t nc_fol;
LockedJoints_t lj_fol;
FoliatedManifold merge(const FoliatedManifold& other) {
FoliatedManifold both;
both.nc = merge_nc(nc, other.nc);
both.nc_path = merge_nc(nc_path, other.nc_path);
std::copy(lj.begin(), lj.end(), both.lj.end());
std::copy(other.lj.begin(), other.lj.end(), both.lj.end());
return both;
}
void addToState(StatePtr_t comp) const;
void addToEdge(EdgePtr_t comp) const;
void specifyFoliation(LevelSetEdgePtr_t lse) const;
bool foliated() const { return !lj_fol.empty() || !nc_fol.empty(); }
bool empty() const { return lj.empty() && nc.empty(); }
};
enum GraspingCase {
NoGrasp = 1 << 0,
GraspOnly = 1 << 1,
WithPreGrasp = 1 << 2
};
enum PlacementCase {
NoPlace = 1 << 3,
PlaceOnly = 1 << 4,
WithPrePlace = 1 << 5
};
struct Rule {
std::vector<std::string> grippers_;
std::vector<std::string> handles_;
bool link_;
Rule() : grippers_(), handles_(), link_(false) {}
};
typedef std::vector<Rule> Rules_t;
/// Create edges according to the case.
/// gCase is a logical OR combination of GraspingCase and PlacementCase
///
/// When an argument is not relevant, use the default constructor
/// of FoliatedManifold
template <int gCase>
Edges_t createEdges(
const std::string& forwName, const std::string& backName,
const StatePtr_t& from, const StatePtr_t& to, const size_type& wForw,
const size_type& wBack, const FoliatedManifold& grasp,
const FoliatedManifold& pregrasp, const FoliatedManifold& place,
const FoliatedManifold& preplace, const bool levelSetGrasp,
const bool levelSetPlace,
const FoliatedManifold& submanifoldDef = FoliatedManifold());
EdgePtr_t createLoopEdge(
const std::string& loopName, const StatePtr_t& state, const size_type& w,
const bool levelSet,
const FoliatedManifold& submanifoldDef = FoliatedManifold());
/// Create a waypoint edge taking into account:
/// \li grasp
/// \li placement
/// \li preplacement
/// Create a waypoint edge taking into account
/// \li grasp
/// \li pregrasp
/// \li placement
void graspManifold(const GripperPtr_t& gripper, const HandlePtr_t& handle,
FoliatedManifold& grasp, FoliatedManifold& pregrasp);
/// The placement foliation constraint is built using
/// hpp::constraints::ConvexShapeMatcherComplement
void strictPlacementManifold(const ImplicitPtr_t placement,
const ImplicitPtr_t preplacement,
const ImplicitPtr_t placementComplement,
FoliatedManifold& place,
FoliatedManifold& preplace);
/// The placement foliation constraint is built locked joints
/// It is faster than strictPlacementManifold but the foliation
/// parametrisation is redundant.
void relaxedPlacementManifold(const ImplicitPtr_t placement,
const ImplicitPtr_t preplacement,
const LockedJoints_t objectLocks,
FoliatedManifold& place,
FoliatedManifold& preplace);
typedef std::tuple<ImplicitPtr_t, ImplicitPtr_t, LockedJoints_t>
PlacementConstraint_t;
typedef std::vector<HandlePtr_t> Handles_t;
typedef std::vector<GripperPtr_t> Grippers_t;
/// Tuple representing an object as follows:
/// \li PlacementConstraint_t constraint to place the object
/// \li Handles_t list of handles of the object
/// \li std::size_t the index of this tuple in Objects_t.
/// \note the index must be unique, as object equallity is checked using this
/// index.
typedef std::tuple<PlacementConstraint_t, Handles_t, std::size_t> Object_t;
typedef std::vector<Object_t> Objects_t;
/// Fill a Graph
///
/// \note It is assumed that a gripper can grasp only one handle and each
/// handle cannot be grasped by several grippers at the same time.
///
/// \param[in,out] graph must be an initialized empty Graph.
void graphBuilder(const ProblemSolverPtr_t& ps, const Objects_t& objects,
const Grippers_t& grippers, GraphPtr_t graph,
const Rules_t& rules = Rules_t());
struct ObjectDef_t {
std::string name;
Strings_t handles, shapes;
};
GraphPtr_t graphBuilder(const ProblemSolverPtr_t& ps,
const std::string& graphName, const Strings_t& griNames,
const std::vector<ObjectDef_t>& objs,
const Strings_t& envNames, const Rules_t& rules,
const value_type& prePlaceWidth = 0.05);
/// \}
} // namespace helper
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_HELPER_HH
include/hpp/manipulation/graph/node-selector.hh deleted 100644 → 0
View file @ 0e448947
// 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/>.
#ifndef HPP_MANIPULATION_GRAPH_NODE_SELECTOR_HH
# define HPP_MANIPULATION_GRAPH_NODE_SELECTOR_HH
#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/graph.hh"
#include "hpp/manipulation/graph/node.hh"
namespace hpp {
namespace manipulation {
namespace graph {
/// This class is used to get the state of a configuration. States have to
/// be ordered because a configuration can be in several states.
class HPP_MANIPULATION_DLLAPI NodeSelector : public GraphComponent
{
public:
/// Create a new NodeSelector.
static NodeSelectorPtr_t create(const std::string& name);
/// Create an empty node
NodePtr_t createNode (const std::string& name, bool waypoint = false,
const int w = 0);
/// Returns the state of a configuration.
NodePtr_t getNode(ConfigurationIn_t config) const;
/// Returns the state of a roadmap node
NodePtr_t getNode(RoadmapNodePtr_t node) const;
/// Returns a list of all the nodes
Nodes_t getNodes () const;
/// Select randomly an outgoing edge of the given node.
virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
/// Should never be called.
void addNumericalConstraint (
const core::NumericalConstraintPtr_t& /* function */,
const SizeIntervals_t& /* passiveDofs */ = SizeIntervals_t ())
{
HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
}
/// Should never be called.
void addLockedJointConstraint
(const core::LockedJoint& /* constraint */)
{
HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
}
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
protected:
/// Initialization of the object.
void init (const NodeSelectorPtr_t& weak);
/// Constructor
NodeSelector (const std::string& name) : GraphComponent (name)
{}
/// Print the object in a stream.
virtual std::ostream& print (std::ostream& os) const;
/// List of the states of one end-effector, ordered by priority.
typedef std::pair <int, NodePtr_t> WeighedNode_t;
typedef std::list <WeighedNode_t> WeighedNodes_t;
WeighedNodes_t orderedStates_;
Nodes_t waypoints_;
private:
/// Weak pointer to itself.
NodeSelectorPtr_t wkPtr_;
}; // Class NodeSelector
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_NODE_SELECTOR_HH
include/hpp/manipulation/graph/node.hh deleted 100644 → 0
View file @ 0e448947
// 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/>.
#ifndef HPP_MANIPULATION_GRAPH_NODE_HH
# define HPP_MANIPULATION_GRAPH_NODE_HH
# include <boost/function.hpp>
#include <hpp/core/locked-joint.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/config-projector.hh>
#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/deprecated.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/fwd.hh"
#include "hpp/manipulation/graph/edge.hh"
#include "hpp/manipulation/graph/graph-component.hh"
namespace hpp {
namespace manipulation {
namespace graph {
/// \addtogroup constraint_graph
/// \{
/// State of an end-effector.
///
/// Nodes of the graph of constraints. There is one
/// graph for each end-effector.
class HPP_MANIPULATION_DLLAPI Node : public GraphComponent
{
public:
typedef boost::function < EdgePtr_t
(const std::string&,
const GraphWkPtr_t&,
const NodeWkPtr_t&, const NodeWkPtr_t&) >
EdgeFactory;
/// Destructor
~Node ();
/// Create a new node.
static NodePtr_t create (const std::string& name);
/// Create a link from this node to the given node.
/// \param w if strictly negative, the edge is not included in the neighbor
/// list. Otherwise, it is included with Weight_t w
EdgePtr_t linkTo (const std::string& name, const NodePtr_t& to,
const size_type& w = 1,
EdgeFactory create = Edge::create);
/// Check whether the configuration is in this state.
/// \code
/// return configConstraint()->isSatisfied (config);
/// \endcode
/// \note You should not use this method to know in which states a
/// configuration is. This only checks if the configuration satisfies
/// the constraints. Instead, use the class NodeSelector.
virtual bool contains (ConfigurationIn_t config) const;
inline bool isWaypoint () const
{
return isWaypoint_;
}
inline void isWaypoint (bool isWaypoint)
{
isWaypoint_ = isWaypoint;
}
/// Get the parent NodeSelector.
NodeSelectorWkPtr_t nodeSelector () const
{
return selector_;
}
/// Set the NodeSelector containing this node.
void nodeSelector (const NodeSelectorWkPtr_t& parent)
{
selector_ = parent;
};
/// Get the neighbors
const Neighbors_t& neighbors() const
{
return neighbors_;
}
void updateWeight (const EdgePtr_t&edge, const Weight_t& w);
/// Constraint to project onto this node.
ConstraintSetPtr_t configConstraint() const;
/// Add core::NumericalConstraint to the component.
virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm,
const SizeIntervals_t& passiveDofs = SizeIntervals_t ())
{
numericalConstraintsForPath_.push_back (nm);
passiveDofsForPath_.push_back (passiveDofs);
}
/// Add core::DifferentiableFunction to the component.
virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
HPP_MANIPULATION_DEPRECATED
{
numericalConstraintsForPath_.push_back (NumericalConstraint::create (function,ineq));
}
/// Insert the numerical constraints in a ConfigProjector
/// \return true is at least one NumericalConstraintPtr_t was inserted.
bool insertNumericalConstraintsForPath (ConfigProjectorPtr_t& proj) const
{
assert (numericalConstraintsForPath_.size () == passiveDofsForPath_.size ());
IntervalsContainer_t::const_iterator itpdofs = passiveDofsForPath_.begin ();
for (NumericalConstraints_t::const_iterator it = numericalConstraintsForPath_.begin();
it != numericalConstraintsForPath_.end(); it++) {
proj->add (*it, *itpdofs);
itpdofs++;
}
return !numericalConstraintsForPath_.empty ();
}
/// Get a reference to the NumericalConstraints_t
const NumericalConstraints_t& numericalConstraintsForPath () const
{
return numericalConstraintsForPath_;
}
/// Print the object in a stream.
std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
protected:
/// Initialize the object.
void init (const NodeWkPtr_t& self);
/// Constructor
Node(const std::string& name);
/// Print the object in a stream.
std::ostream& print (std::ostream& os) const;
virtual void populateTooltip (dot::Tooltip& tp) const;
private:
/// List of possible motions from this state (i.e. the outgoing
/// vertices).
Neighbors_t neighbors_;
std::vector <EdgePtr_t> hiddenNeighbors_;
/// Set of constraints to be statisfied.
typedef Cache < ConstraintSetPtr_t > Constraint_t;
Constraint_t* configConstraints_;
/// Stores the numerical constraints for path.
NumericalConstraints_t numericalConstraintsForPath_;
IntervalsContainer_t passiveDofsForPath_;
/// A selector that will implement the selection of the next state.
NodeSelectorWkPtr_t selector_;
/// Weak pointer to itself.
NodeWkPtr_t wkPtr_;
bool isWaypoint_;
}; // class Node
/// \}
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_NODE_HH
include/hpp/manipulation/graph/state-selector.hh 0 → 100644
View file @ 539aab52
// Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_STATE_SELECTOR_HH
#define HPP_MANIPULATION_GRAPH_STATE_SELECTOR_HH
#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/graph.hh"
#include "hpp/manipulation/graph/state.hh"
namespace hpp {
namespace manipulation {
namespace graph {
/// This class is used to get the state of a configuration. States have to
/// be ordered because a configuration can be in several states.
class HPP_MANIPULATION_DLLAPI StateSelector {
public:
virtual ~StateSelector() {};
/// Create a new StateSelector.
static StateSelectorPtr_t create(const std::string& name);
const std::string& name() const { return name_; }
/// Create an empty state
StatePtr_t createState(const std::string& name, bool waypoint = false,
const int w = 0);
/// Returns the state of a configuration.
StatePtr_t getState(ConfigurationIn_t config) const;
/// Returns the state of a roadmap state
StatePtr_t getState(RoadmapNodePtr_t node) const;
/// Returns a list of all the states
States_t getStates() const;
/// Returns a list of all the states
States_t getWaypointStates() const;
/// Select randomly an outgoing edge of the given state.
virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
/// Print the object in a stream.
virtual std::ostream& dotPrint(
std::ostream& os,
dot::DrawingAttributes da = dot::DrawingAttributes()) const;
/// Set the parent graph.
void parentGraph(const GraphWkPtr_t& parent);
/// Set the parent graph.
GraphPtr_t parentGraph() const;
protected:
/// Initialization of the object.
void init(const StateSelectorPtr_t& weak);
/// Constructor
StateSelector(const std::string& name) : name_(name) {}
/// Print the object in a stream.
virtual std::ostream& print(std::ostream& os) const;
/// List of the states of one end-effector, ordered by priority.
typedef std::pair<int, StatePtr_t> WeighedState_t;
typedef std::list<WeighedState_t> WeighedStates_t;
WeighedStates_t orderedStates_;
States_t waypoints_;
private:
/// Name of the component.
std::string name_;
/// A weak pointer to the parent graph.
GraphWkPtr_t graph_;
/// Weak pointer to itself.
StateSelectorPtr_t wkPtr_;
friend std::ostream& operator<<(std::ostream& os, const StateSelector& ss);
}; // Class StateSelector
inline std::ostream& operator<<(std::ostream& os, const StateSelector& ss) {
return ss.print(os);
}
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_STATE_SELECTOR_HH
include/hpp/manipulation/graph/state.hh 0 → 100644
View file @ 539aab52
// Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_STATE_HH
#define HPP_MANIPULATION_GRAPH_STATE_HH
#include <functional>
#include <hpp/constraints/implicit.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/core/constraint-set.hh>
#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/edge.hh"
#include "hpp/manipulation/graph/fwd.hh"
#include "hpp/manipulation/graph/graph-component.hh"
namespace hpp {
namespace manipulation {
using constraints::Implicit;
using constraints::ImplicitPtr_t;
namespace graph {
/// \addtogroup constraint_graph
/// \{
/// State of an end-effector.
///
/// States of the graph of constraints. There is one
/// graph for each end-effector.
class HPP_MANIPULATION_DLLAPI State : public GraphComponent {
public:
typedef std::function<EdgePtr_t(const std::string&, const GraphWkPtr_t&,
const StateWkPtr_t&, const StateWkPtr_t&)>
EdgeFactory;
/// Destructor
~State();
/// Create a new state.
static StatePtr_t create(const std::string& name);
/// Create a link from this state to the given state.
/// \param w if strictly negative, the edge is not included in the neighbor
/// list. Otherwise, it is included with Weight_t w
EdgePtr_t linkTo(const std::string& name, const StatePtr_t& to,
const size_type& w = 1, EdgeFactory create = Edge::create);
/// Check whether the configuration is in this state.
/// \code
/// return configConstraint()->isSatisfied (config);
/// \endcode
/// \note You should not use this method to know in which states a
/// configuration is. This only checks if the configuration satisfies
/// the constraints. Instead, use the class StateSelector.
virtual bool contains(ConfigurationIn_t config) const;
inline bool isWaypoint() const { return isWaypoint_; }
inline void isWaypoint(bool isWaypoint) { isWaypoint_ = isWaypoint; }
/// Get the parent StateSelector.
StateSelectorWkPtr_t stateSelector() const { return selector_; }
/// Set the StateSelector containing this state.
void stateSelector(const StateSelectorWkPtr_t& parent) {
selector_ = parent;
};
/// Get the neighbors
const Neighbors_t& neighbors() const { return neighbors_; }
/// Get the neighbors
Edges_t neighborEdges() const { return neighbors_.values(); }
/// Get the hidden neighbors
/// It is a vector of transitions outgoing from this state and that are
/// included in a waypoint edge.
const Edges_t& hiddenNeighbors() const { return hiddenNeighbors_; }
/// Set weight of edge starting from this state.
void updateWeight(const EdgePtr_t& edge, const Weight_t& w);
/// Get weight of edge starting from this state.
Weight_t getWeight(const EdgePtr_t& edge);
/// Constraint to project onto this state.
ConstraintSetPtr_t configConstraint() const {
throwIfNotInitialized();
return configConstraints_;
}
/// Add constraint to the state
/// Call the parent implementation.
/// \throw std::logic_error if the constraint is parameterizable
/// (contains at least one Equality comparison type).
virtual void addNumericalConstraint(const ImplicitPtr_t& numConstraint);
/// Add a constraint for paths that lie in this state.
virtual void addNumericalConstraintForPath(const ImplicitPtr_t& nm) {
invalidate();
numericalConstraintsForPath_.push_back(nm);
}
/// Insert the numerical constraints in a ConfigProjector
/// \return true is at least one ImplicitPtr_t was inserted.
bool insertNumericalConstraintsForPath(ConfigProjectorPtr_t& proj) const {
for (const auto& nc : numericalConstraintsForPath_) proj->add(nc);
return !numericalConstraintsForPath_.empty();
}
/// Get a reference to the NumericalConstraints_t
const NumericalConstraints_t& numericalConstraintsForPath() const {
return numericalConstraintsForPath_;
}
/// Print the object in a stream.
std::ostream& dotPrint(std::ostream& os, dot::DrawingAttributes da =
dot::DrawingAttributes()) const;
protected:
/// Initialize the object.
void init(const StateWkPtr_t& self);
/// Constructor
State(const std::string& name);
/// Print the object in a stream.
std::ostream& print(std::ostream& os) const;
virtual void populateTooltip(dot::Tooltip& tp) const;
virtual void initialize();
private:
/// List of possible motions from this state (i.e. the outgoing
/// vertices).
Neighbors_t neighbors_;
Edges_t hiddenNeighbors_;
/// Set of constraints to be statisfied.
ConstraintSetPtr_t configConstraints_;
/// Stores the numerical constraints for path.
NumericalConstraints_t numericalConstraintsForPath_;
/// A selector that will implement the selection of the next state.
StateSelectorWkPtr_t selector_;
/// Weak pointer to itself.
StateWkPtr_t wkPtr_;
bool isWaypoint_;
}; // class State
/// \}
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_STATE_HH
include/hpp/manipulation/graph/statistics.hh
View file @ 539aab52
// 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/>.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_STATISTICS_HH
# define HPP_MANIPULATION_GRAPH_STATISTICS_HH
# include <hpp/util/debug.hh>
#define HPP_MANIPULATION_GRAPH_STATISTICS_HH
# include <hpp/statistics/bin.hh>
#include <hpp/manipulation/roadmap-node.hh>
#include <hpp/statistics/bin.hh>
#include <hpp/util/debug.hh>
# include "hpp/manipulation/config.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/graph.hh"
# include "hpp/manipulation/graph/node.hh"
# include <hpp/manipulation/roadmap-node.hh>
#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/graph.hh"
#include "hpp/manipulation/graph/state.hh"
namespace hpp {
namespace manipulation {
namespace graph {
/// This class is used to do statistics on the roadmap.
/// It keeps a track of which leaves of a foliation have been visited.
class HPP_MANIPULATION_DLLAPI LeafBin : public ::hpp::statistics::Bin
{
public :
typedef ::hpp::statistics::Bin Parent;
typedef std::list <RoadmapNodePtr_t> RoadmapNodes_t;
namespace manipulation {
namespace graph {
/// This class is used to do statistics on the roadmap.
/// It keeps a track of which leaves of a foliation have been visited.
class HPP_MANIPULATION_DLLAPI LeafBin : public ::hpp::statistics::Bin {
public:
typedef ::hpp::statistics::Bin Parent;
typedef std::list<RoadmapNodePtr_t> RoadmapNodes_t;
LeafBin(const vector_t& v, value_type* threshold_);
void push_back(const RoadmapNodePtr_t& n);
bool operator<(const LeafBin& rhs) const;
bool operator==(const LeafBin& rhs) const;
const vector_t& value() const;
LeafBin(const vector_t& v, value_type* threshold_);
std::ostream& print(std::ostream& os) const;
void push_back(const RoadmapNodePtr_t& n);
unsigned int numberOfObsOutOfConnectedComponent(
const core::ConnectedComponentPtr_t& cc) const;
bool operator<(const LeafBin& rhs) const;
const RoadmapNodes_t& nodes() const;
bool operator==(const LeafBin& rhs) const;
private:
vector_t value_;
const vector_t& value () const;
RoadmapNodes_t nodes_;
std::ostream& print (std::ostream& os) const;
value_type* thr_;
unsigned int numberOfObsOutOfConnectedComponent (const core::ConnectedComponentPtr_t& cc) const;
std::ostream& printValue(std::ostream& os) const;
};
const RoadmapNodes_t& nodes () const;
/// This class is used to do statistics on the roadmap.
/// It keeps a track of which nodes of the constraint graph have been visited.
class HPP_MANIPULATION_DLLLOCAL NodeBin : public ::hpp::statistics::Bin {
public:
typedef ::hpp::statistics::Bin Parent;
NodeBin(const StatePtr_t& n);
private:
vector_t value_;
void push_back(const RoadmapNodePtr_t& n);
RoadmapNodes_t nodes_;
bool operator<(const NodeBin& rhs) const;
value_type* thr_;
bool operator==(const NodeBin& rhs) const;
std::ostream& printValue (std::ostream& os) const;
};
const StatePtr_t& state() const;
/// This class is used to do statistics on the roadmap.
/// It keeps a track of which nodes of the constraint graph have been visited.
class HPP_MANIPULATION_DLLLOCAL NodeBin : public ::hpp::statistics::Bin
{
public :
typedef ::hpp::statistics::Bin Parent;
NodeBin(const NodePtr_t& n);
std::ostream& print(std::ostream& os) const;
void push_back(const RoadmapNodePtr_t& n);
private:
StatePtr_t state_;
bool operator<(const NodeBin& rhs) const;
typedef std::list<RoadmapNodePtr_t> RoadmapNodes_t;
RoadmapNodes_t roadmapNodes_;
bool operator==(const NodeBin& rhs) const;
std::ostream& printValue(std::ostream& os) const;
};
const NodePtr_t& node () const;
class HPP_MANIPULATION_DLLLOCAL Histogram {
public:
virtual ~Histogram() {};
std::ostream& print (std::ostream& os) const;
virtual void add(const RoadmapNodePtr_t& node) = 0;
private:
NodePtr_t node_;
virtual HistogramPtr_t clone() const = 0;
typedef std::list <RoadmapNodePtr_t> RoadmapNodes_t;
RoadmapNodes_t roadmapNodes_;
virtual void clear() = 0;
};
std::ostream& printValue (std::ostream& os) const;
};
/// This class represents a foliation of a submanifold of the configuration
/// space.
/// Such a foliation is defined by the two following functions:
/// \li a condition $f$ such that the submanifold is
/// $\mathcal{M}= \left{q \in \mathbb{C} | f(q)=0 \right}$
/// \li a parametrizer $g$ such that the leaf of this foliation is a level
/// set of $g$.
class HPP_MANIPULATION_DLLAPI Foliation {
public:
/// Whether the configuration is the submanifold $\mathcal{M}$
bool contains(ConfigurationIn_t q) const;
/// Whether the configuration is the submanifold $\mathcal{M}$
vector_t parameter(ConfigurationIn_t q) const;
class HPP_MANIPULATION_DLLLOCAL Histogram
{
public:
virtual void add (const RoadmapNodePtr_t& node) = 0;
void condition(const ConstraintSetPtr_t c);
ConstraintSetPtr_t condition() const;
void parametrizer(const ConstraintSetPtr_t p);
ConstraintSetPtr_t parametrizer() const;
virtual HistogramPtr_t clone () const = 0;
private:
// condition_ contains the constraints defining the submanifold
// containing all the leaf.
// parametrizer_ contains the constraints providing a parametrization
// of the foliation.
ConstraintSetPtr_t condition_, parametrizer_;
};
virtual void clear () = 0;
};
class HPP_MANIPULATION_DLLAPI LeafHistogram
: public ::hpp::statistics::Statistics<LeafBin>,
public Histogram {
public:
typedef ::hpp::statistics::Statistics<LeafBin> Parent;
/// This class represents a foliation of a submanifold of the configuration
/// space.
/// Such a foliation is defined by the two following functions:
/// \li a condition $f$ such that the submanifold is
/// $\mathcal{M}= \left{q \in \mathbb{C} | f(q)=0 \right}$
/// \li a parametrizer $g$ such that the leaf of this foliation is a level
/// set of $g$.
class HPP_MANIPULATION_DLLAPI Foliation
{
public:
/// Whether the configuration is the submanifold $\mathcal{M}$
bool contains (ConfigurationIn_t q) const;
/// Whether the configuration is the submanifold $\mathcal{M}$
vector_t parameter (ConfigurationIn_t q) const;
static LeafHistogramPtr_t create(const Foliation f);
void condition (const ConstraintSetPtr_t c);
ConstraintSetPtr_t condition () const;
void parametrizer (const ConstraintSetPtr_t p);
ConstraintSetPtr_t parametrizer () const;
/// Insert an occurence of a value in the histogram
void add(const RoadmapNodePtr_t& n);
private:
// condition_ contains the constraints defining the submanifold
// containing all the leaf.
// parametrizer_ contains the constraints providing a parametrization
// of the foliation.
ConstraintSetPtr_t condition_, parametrizer_;
};
std::ostream& print(std::ostream& os) const;
class HPP_MANIPULATION_DLLAPI LeafHistogram : public ::hpp::statistics::Statistics < LeafBin >
, public Histogram
{
public:
typedef ::hpp::statistics::Statistics < LeafBin > Parent;
virtual HistogramPtr_t clone() const;
static LeafHistogramPtr_t create (const Foliation f);
statistics::DiscreteDistribution<RoadmapNodePtr_t>
getDistribOutOfConnectedComponent(
const core::ConnectedComponentPtr_t& cc) const;
/// Insert an occurence of a value in the histogram
void add (const RoadmapNodePtr_t& n);
statistics::DiscreteDistribution<RoadmapNodePtr_t> getDistrib() const;
std::ostream& print (std::ostream& os) const;
void clear() { Parent::clear(); }
virtual HistogramPtr_t clone () const;
const Foliation& foliation() const { return f_; }
statistics::DiscreteDistribution < RoadmapNodePtr_t > getDistribOutOfConnectedComponent (
const core::ConnectedComponentPtr_t& cc) const;
protected:
/// Constructor
/// \param state defines the submanifold containing the foliation.
/// \param constraint The constraint that create the foliation being
/// studied.
LeafHistogram(const Foliation f);
statistics::DiscreteDistribution < RoadmapNodePtr_t > getDistrib () const;
private:
Foliation f_;
void clear () { Parent::clear(); }
/// Threshold used for equality between offset values.
value_type threshold_;
};
const Foliation& foliation () const {
return f_;
}
class HPP_MANIPULATION_DLLLOCAL StateHistogram
: public ::hpp::statistics::Statistics<NodeBin>,
public Histogram {
public:
typedef ::hpp::statistics::Statistics<NodeBin> Parent;
/// Constructor
/// \param graph The constraint graph used to get the states from
/// a configuration.
StateHistogram(const graph::GraphPtr_t& graph);
protected:
/// Constructor
/// \param node defines the submanifold containing the foliation.
/// \param constraint The constraint that create the foliation being
/// studied.
LeafHistogram (const Foliation f);
/// Insert an occurence of a value in the histogram
void add(const RoadmapNodePtr_t& n);
private:
Foliation f_;
std::ostream& print(std::ostream& os) const;
/// Threshold used for equality between offset values.
value_type threshold_;
};
const graph::GraphPtr_t& constraintGraph() const;
class HPP_MANIPULATION_DLLLOCAL NodeHistogram : public ::hpp::statistics::Statistics < NodeBin >
, public Histogram
{
public:
typedef ::hpp::statistics::Statistics < NodeBin > Parent;
/// Constructor
/// \param graph The constraint graph used to get the nodes from
/// a configuration.
NodeHistogram (const graph::GraphPtr_t& graph);
virtual HistogramPtr_t clone() const;
/// Insert an occurence of a value in the histogram
void add (const RoadmapNodePtr_t& n);
void clear() { Parent::clear(); }
std::ostream& print (std::ostream& os) const;
const graph::GraphPtr_t& constraintGraph () const;
virtual HistogramPtr_t clone () const;
void clear () { Parent::clear(); }
private:
/// The constraint graph
graph::GraphPtr_t graph_;
};
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_STATISTICS_HH
private:
/// The constraint graph
graph::GraphPtr_t graph_;
};
typedef shared_ptr<StateHistogram> NodeHistogramPtr_t;
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_STATISTICS_HH
include/hpp/manipulation/graph/validation.hh 0 → 100644
View file @ 539aab52
// Copyright (c) 2019, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_VALIDATION_REPORT_HH
#define HPP_MANIPULATION_GRAPH_VALIDATION_REPORT_HH
#include <hpp/core/validation-report.hh>
#include <hpp/manipulation/config.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/manipulation/graph/fwd.hh>
#include <hpp/manipulation/graph/graph.hh>
#include <string>
#include <vector>
namespace hpp {
namespace manipulation {
namespace graph {
/// \addtogroup constraint_graph
/// \{
/// Check that graph components are valid.
///
/// A stringified validation report can be obtained via
/// Validation::print or operator<< (std::ostream&, const Validation&).
class HPP_MANIPULATION_DLLAPI Validation {
public:
typedef std::vector<std::string> Collision;
typedef std::vector<Collision> CollisionList;
typedef std::map<std::string, CollisionList> CollisionMap;
Validation(const core::ProblemPtr_t& problem) : problem_(problem) {}
void clear() {
warnings_.clear();
errors_.clear();
}
bool hasWarnings() const { return !warnings_.empty(); }
bool hasErrors() const { return !errors_.empty(); }
virtual std::ostream& print(std::ostream& os) const;
/// Validate a graph component.
/// It dynamically casts in order to call the right function among
/// the validation method below.
///
/// \return true if the component could not be proven infeasible.
/// \note Even if true is returned, the report can contain warnings.
bool validate(const GraphComponentPtr_t& comp);
/// Validate a state
/// \return true if the state could not be proven infeasible.
/// \note Even if true is returned, the report can contain warnings.
bool validateState(const StatePtr_t& state);
/// Validate an edge
/// \return true if the edge could not be proven infeasible.
/// \note Even if true is returned, the report can contain warnings.
bool validateEdge(const EdgePtr_t& edge);
/// Validate an graph
/// \return true if no component of the graph could not be proven infeasible.
/// \note Even if true is returned, the report can contain warnings.
bool validateGraph(const GraphPtr_t& graph);
CollisionList getCollisionsForNode(const std::string& nodeName) {
return collisions_[nodeName];
}
private:
void addWarning(const GraphComponentPtr_t& c, const std::string& w) {
warnings_.push_back(Message(c, w));
}
void addError(const GraphComponentPtr_t& c, const std::string& w) {
errors_.push_back(Message(c, w));
}
void addCollision(const GraphComponentPtr_t& c, const std::string& obj1,
const std::string& obj2) {
Collision coll = Collision{obj1, obj2};
collisions_[c->name()].push_back(coll);
}
typedef std::pair<GraphComponentPtr_t, std::string> Message;
std::vector<Message> warnings_, errors_;
CollisionMap collisions_;
core::ProblemPtr_t problem_;
};
inline std::ostream& operator<<(std::ostream& os, const Validation& v) {
return v.print(os);
}
/// \}
} // namespace graph
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_VALIDATION_REPORT_HH