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

Target

Select target project
  • gsaurel/hpp-manipulation
  • humanoid-path-planner/hpp-manipulation
2 results
Show changes
Expand all 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 @@ ...@@ -3,107 +3,110 @@
/// Authors: Florent Lamiraux, Joseph Mirabel /// Authors: Florent Lamiraux, Joseph Mirabel
/// ///
/// ///
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 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 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// General Lesser Public License for more details. You should have // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// received a copy of the GNU Lesser General Public License along with // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// hpp-manipulation. If not, see // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// <http://www.gnu.org/licenses/>. // 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 #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/core/container.hh> #include <hpp/manipulation/config.hh>
#include <hpp/manipulation/fwd.hh>
# include "hpp/manipulation/fwd.hh" #include <hpp/pinocchio/humanoid-robot.hh>
# include "hpp/manipulation/config.hh"
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
/// Device with handles. /// Device with handles.
/// ///
/// As a deriving class of hpp::model::HumanoidRobot, /// As a deriving class of hpp::pinocchio::HumanoidRobot,
/// it is compatible with hpp::model::urdf::loadHumanoidRobot /// it is compatible with hpp::pinocchio::urdf::loadHumanoidRobot
/// ///
/// This class also contains model::Gripper, Handle and \ref JointAndTriangles_t /// This class also contains pinocchio::Gripper, Handle and \ref
class HPP_MANIPULATION_DLLAPI Device : /// JointAndShapes_t
public model::HumanoidRobot, class HPP_MANIPULATION_DLLAPI Device : public pinocchio::HumanoidRobot {
public core::Containers< public:
boost::mpl::vector < HandlePtr_t, typedef pinocchio::HumanoidRobot Parent_t;
model::GripperPtr_t,
JointAndShapes_t, /// Constructor
model::JointVector_t> > /// \param name of the new instance,
{ static DevicePtr_t create(const std::string& name) {
public: Device* ptr = new Device(name);
typedef model::HumanoidRobot Parent_t; DevicePtr_t shPtr(ptr);
ptr->init(shPtr);
typedef core::Containers< return shPtr;
boost::mpl::vector < HandlePtr_t, }
model::GripperPtr_t,
JointAndShapes_t, DevicePtr_t self() const { return self_.lock(); }
model::JointVector_t> > Containers_t;
/// Print object in a stream
/// Constructor virtual std::ostream& print(std::ostream& os) const;
/// \param name of the new instance,
static DevicePtr_t create (const std::string& name) void setRobotRootPosition(const std::string& robotName,
{ const Transform3s& positionWRTParentJoint);
Device* ptr = new Device (name);
DevicePtr_t shPtr (ptr); virtual pinocchio::DevicePtr_t clone() const;
ptr->init (shPtr);
return shPtr; std::vector<std::string> robotNames() const;
}
FrameIndices_t robotFrames(const std::string& robotName) const;
/// Print object in a stream
virtual std::ostream& print (std::ostream& os) const; void removeJoints(const std::vector<std::string>& jointNames,
Configuration_t referenceConfig);
/// \name Collisions
/// \{ core::Container<HandlePtr_t> handles;
core::Container<GripperPtr_t> grippers;
/// Cache joint vector core::Container<JointAndShapes_t> jointAndShapes;
void prepareInsertRobot ()
{ protected:
didPrepare_ = true; /// Constructor
jointCache_ = getJointVector (); /// \param name of the new instance,
} /// \param robot Robots that manipulate objects,
/// \param objects Set of objects manipulated by the robot.
/// Add collisions Device(const std::string& name) : Parent_t(name) {}
/// between the joint vector cache initialized by prepareInsertRobot()
/// add the current Robot. void init(const DeviceWkPtr_t& self) {
/// When creating a robot from several URDF files, this enables Parent_t::init(self);
/// collisions between joints from different files. self_ = self;
void didInsertRobot (const std::string& name); }
/// \} void initCopy(const DeviceWkPtr_t& self, const Device& other) {
Parent_t::initCopy(self, other);
protected: self_ = self;
/// Constructor }
/// \param name of the new instance,
/// \param robot Robots that manipulate objects, /// For serialization only
/// \param objects Set of objects manipulated by the robot. Device() {}
Device (const std::string& name) :
Parent_t (name), jointCache_ (), didPrepare_ (false) private:
{} DeviceWkPtr_t self_;
void init (const DeviceWkPtr_t& self) HPP_SERIALIZABLE();
{ }; // class Device
Parent_t::init (self); } // namespace manipulation
self_ = self; } // namespace hpp
}
BOOST_CLASS_EXPORT_KEY(hpp::manipulation::Device)
private:
DeviceWkPtr_t self_; #endif // HPP_MANIPULATION_DEVICE_HH
model::JointVector_t jointCache_;
bool didPrepare_;
}; // class Device
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_DEVICE_HH
include/hpp/manipulation/fwd.hh
View file @ 539aab52
...@@ -3,132 +3,173 @@ ...@@ -3,132 +3,173 @@
// Authors: Florent Lamiraux, Joseph Mirabel // Authors: Florent Lamiraux, Joseph Mirabel
// //
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 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 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// General Lesser Public License for more details. You should have // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// received a copy of the GNU Lesser General Public License along with // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// hpp-manipulation. If not, see // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// <http://www.gnu.org/licenses/>. // 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 #ifndef HPP_MANIPULATION_FWD_HH
# define HPP_MANIPULATION_FWD_HH #define HPP_MANIPULATION_FWD_HH
# include <map> #include <hpp/core/fwd.hh>
# include <hpp/core/fwd.hh> #include <map>
# include <hpp/fcl/shape/geometric_shapes.h>
# include <hpp/manipulation/deprecated.hh>
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
HPP_PREDEF_CLASS (Device); HPP_PREDEF_CLASS(Device);
typedef boost::shared_ptr <Device> DevicePtr_t; typedef shared_ptr<Device> DevicePtr_t;
typedef boost::shared_ptr <const Device> DeviceConstPtr_t; typedef shared_ptr<const Device> DeviceConstPtr_t;
typedef model::Joint Joint; typedef pinocchio::Joint Joint;
typedef model::JointPtr_t JointPtr_t; typedef pinocchio::JointPtr_t JointPtr_t;
typedef model::JointConstPtr_t JointConstPtr_t; typedef pinocchio::JointIndex JointIndex;
typedef model::Transform3f Transform3f; typedef std::vector<JointIndex> JointIndices_t;
typedef model::Configuration_t Configuration_t; typedef pinocchio::FrameIndex FrameIndex;
typedef model::ConfigurationIn_t ConfigurationIn_t; typedef std::vector<pinocchio::FrameIndex> FrameIndices_t;
typedef model::ConfigurationOut_t ConfigurationOut_t; typedef pinocchio::Configuration_t Configuration_t;
typedef boost::shared_ptr < model::Configuration_t > ConfigurationPtr_t; typedef pinocchio::ConfigurationIn_t ConfigurationIn_t;
typedef model::GripperPtr_t GripperPtr_t; typedef pinocchio::ConfigurationOut_t ConfigurationOut_t;
HPP_PREDEF_CLASS (AxialHandle); typedef pinocchio::GripperPtr_t GripperPtr_t;
typedef boost::shared_ptr <AxialHandle> AxialHandlePtr_t; typedef pinocchio::LiegroupElement LiegroupElement;
HPP_PREDEF_CLASS (Handle); typedef pinocchio::LiegroupSpace LiegroupSpace;
typedef boost::shared_ptr <Handle> HandlePtr_t; typedef pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t;
HPP_PREDEF_CLASS (Object); HPP_PREDEF_CLASS(AxialHandle);
typedef boost::shared_ptr <Object> ObjectPtr_t; typedef shared_ptr<AxialHandle> AxialHandlePtr_t;
typedef boost::shared_ptr <const Object> ObjectConstPtr_t; HPP_PREDEF_CLASS(Handle);
HPP_PREDEF_CLASS (ProblemSolver); typedef shared_ptr<Handle> HandlePtr_t;
typedef ProblemSolver* ProblemSolverPtr_t; HPP_PREDEF_CLASS(Object);
HPP_PREDEF_CLASS (Problem); typedef shared_ptr<Object> ObjectPtr_t;
typedef Problem* ProblemPtr_t; typedef shared_ptr<const Object> ObjectConstPtr_t;
HPP_PREDEF_CLASS (Roadmap); HPP_PREDEF_CLASS(ProblemSolver);
typedef boost::shared_ptr <Roadmap> RoadmapPtr_t; typedef ProblemSolver* ProblemSolverPtr_t;
HPP_PREDEF_CLASS (RoadmapNode); HPP_PREDEF_CLASS(Problem);
typedef RoadmapNode* RoadmapNodePtr_t; typedef shared_ptr<Problem> ProblemPtr_t;
HPP_PREDEF_CLASS (ConnectedComponent); typedef shared_ptr<const Problem> ProblemConstPtr_t;
typedef boost::shared_ptr<ConnectedComponent> ConnectedComponentPtr_t; HPP_PREDEF_CLASS(Roadmap);
HPP_PREDEF_CLASS (WeighedDistance); typedef shared_ptr<Roadmap> RoadmapPtr_t;
typedef boost::shared_ptr<WeighedDistance> WeighedDistancePtr_t; HPP_PREDEF_CLASS(RoadmapNode);
typedef constraints::RelativeOrientation RelativeOrientation; typedef RoadmapNode* RoadmapNodePtr_t;
typedef constraints::RelativePosition RelativePosition; typedef std::vector<RoadmapNodePtr_t> RoadmapNodes_t;
typedef constraints::RelativeOrientationPtr_t RelativeOrientationPtr_t; HPP_PREDEF_CLASS(ConnectedComponent);
typedef constraints::RelativePositionPtr_t RelativePositionPtr_t; typedef shared_ptr<ConnectedComponent> ConnectedComponentPtr_t;
typedef constraints::RelativeTransformation RelativeTransformation; HPP_PREDEF_CLASS(LeafConnectedComp);
typedef constraints::RelativeTransformationPtr_t typedef shared_ptr<LeafConnectedComp> LeafConnectedCompPtr_t;
RelativeTransformationPtr_t; typedef shared_ptr<const LeafConnectedComp> LeafConnectedCompConstPtr_t;
typedef model::value_type value_type; typedef std::set<LeafConnectedCompPtr_t> LeafConnectedComps_t;
typedef model::size_type size_type; HPP_PREDEF_CLASS(WeighedLeafConnectedComp);
typedef model::Transform3f Transform3f; typedef shared_ptr<WeighedLeafConnectedComp> WeighedLeafConnectedCompPtr_t;
typedef model::vector_t vector_t; HPP_PREDEF_CLASS(WeighedDistance);
typedef model::vectorIn_t vectorIn_t; typedef shared_ptr<WeighedDistance> WeighedDistancePtr_t;
typedef model::vectorOut_t vectorOut_t; typedef constraints::RelativeOrientation RelativeOrientation;
HPP_PREDEF_CLASS (ManipulationPlanner); typedef constraints::RelativePosition RelativePosition;
typedef boost::shared_ptr < ManipulationPlanner > ManipulationPlannerPtr_t; typedef constraints::RelativeOrientationPtr_t RelativeOrientationPtr_t;
HPP_PREDEF_CLASS (GraphPathValidation); typedef constraints::RelativePositionPtr_t RelativePositionPtr_t;
typedef boost::shared_ptr < GraphPathValidation > GraphPathValidationPtr_t; typedef constraints::RelativeTransformation RelativeTransformation;
HPP_PREDEF_CLASS (GraphSteeringMethod); typedef constraints::RelativeTransformationR3xSO3 RelativeTransformationR3xSO3;
typedef boost::shared_ptr < GraphSteeringMethod > GraphSteeringMethodPtr_t; typedef constraints::RelativeTransformationPtr_t RelativeTransformationPtr_t;
typedef core::PathOptimizer PathOptimizer; typedef core::value_type value_type;
typedef core::PathOptimizerPtr_t PathOptimizerPtr_t; typedef core::size_type size_type;
HPP_PREDEF_CLASS (GraphOptimizer); typedef core::Transform3s Transform3s;
typedef boost::shared_ptr < GraphOptimizer > GraphOptimizerPtr_t; typedef core::vector_t vector_t;
HPP_PREDEF_CLASS (GraphNodeOptimizer); typedef core::vectorIn_t vectorIn_t;
typedef boost::shared_ptr < GraphNodeOptimizer > GraphNodeOptimizerPtr_t; typedef core::vectorOut_t vectorOut_t;
typedef core::PathProjectorPtr_t PathProjectorPtr_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 core::Shape_t Shape_t;
typedef std::vector <ObjectPtr_t> Objects_t; typedef core::JointAndShape_t JointAndShape_t;
typedef std::map <JointConstPtr_t, JointPtr_t> JointMap_t; typedef core::JointAndShapes_t JointAndShapes_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 std::list < NumericalConstraintPtr_t > NumericalConstraints_t; typedef std::list<std::string> StringList_t;
typedef std::pair< GripperPtr_t, HandlePtr_t> Grasp_t; typedef std::vector<std::string> Strings_t;
typedef boost::shared_ptr <Grasp_t> GraspPtr_t;
typedef std::map <NumericalConstraintPtr_t, GraspPtr_t> GraspsMap_t;
typedef std::vector<core::vector3_t> Shape_t; namespace pathOptimization {
typedef std::pair <JointPtr_t, Shape_t> JointAndShape_t; HPP_PREDEF_CLASS(SmallSteps);
typedef std::list <JointAndShape_t> JointAndShapes_t; typedef shared_ptr<SmallSteps> SmallStepsPtr_t;
typedef std::list <std::string> StringList_t; HPP_PREDEF_CLASS(Keypoints);
typedef HPP_MANIPULATION_DEPRECATED Shape_t Triangle; typedef shared_ptr<Keypoints> KeypointsPtr_t;
typedef HPP_MANIPULATION_DEPRECATED JointAndShape_t JointAndTriangle_t; } // namespace pathOptimization
typedef HPP_MANIPULATION_DEPRECATED JointAndShapes_t JointAndTriangles_t;
namespace pathOptimization { namespace problemTarget {
HPP_PREDEF_CLASS (SmallSteps); HPP_PREDEF_CLASS(State);
typedef boost::shared_ptr < SmallSteps > SmallStepsPtr_t; typedef shared_ptr<State> StatePtr_t;
} // namespace pathOptimization } // namespace problemTarget
} // namespace manipulation } // namespace manipulation
} // namespace hpp } // 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 // Copyright (c) 2015, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 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 // 2. Redistributions in binary form must reproduce the above copyright
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // notice, this list of conditions and the following disclaimer in the
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // documentation and/or other materials provided with the distribution.
// General Lesser Public License for more details. You should have //
// received a copy of the GNU Lesser General Public License along with // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. // "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 #ifndef HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
# define 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>
# include <hpp/manipulation/fwd.hh> #include <hpp/core/path-optimizer.hh>
# include <hpp/manipulation/graph/fwd.hh> #include <hpp/core/path-vector.hh>
# include <hpp/manipulation/config.hh> #include <hpp/core/path.hh>
# include <hpp/manipulation/constraint-set.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 hpp {
namespace manipulation { namespace manipulation {
using hpp::core::Path; using hpp::core::Path;
using hpp::core::PathPtr_t; using hpp::core::PathPtr_t;
using hpp::core::PathVector; using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t; using hpp::core::PathVectorPtr_t;
/// \addtogroup path_optimization /// \addtogroup path_optimization
/// \{ /// \{
/// Path optimizer that recompute the edge parameter of the constraints /// Path optimizer that recompute the edge parameter of the constraints
/// ///
/// This class encapsulates another path optimizer class. This optimizer /// This class encapsulates another path optimizer class. This optimizer
/// calls the inner optimizer on every subpaths with the same set of /// calls the inner optimizer on every subpaths with the same set of
/// constraints. /// constraints.
class HPP_MANIPULATION_DLLAPI GraphNodeOptimizer : public PathOptimizer class HPP_MANIPULATION_DLLAPI GraphNodeOptimizer : public PathOptimizer {
{ public:
public: static GraphNodeOptimizerPtr_t create(const core::ProblemConstPtr_t& problem);
static GraphNodeOptimizerPtr_t create (const core::Problem& problem);
virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path); virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
protected: protected:
/// Constructor /// Constructor
GraphNodeOptimizer (const core::Problem& problem) : GraphNodeOptimizer(const core::ProblemConstPtr_t& problem)
PathOptimizer (problem) : PathOptimizer(problem) {}
{}
private: private:
}; };
/// \} /// \}
} // namespace manipulation } // namespace manipulation
} // namespace hpp } // 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 // Copyright (c) 2015, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // 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 #ifndef HPP_MANIPULATION_GRAPHOPTIMIZER_HH
# define HPP_MANIPULATION_GRAPHOPTIMIZER_HH #define HPP_MANIPULATION_GRAPHOPTIMIZER_HH
# include <hpp/core/path.hh> #include <hpp/core/path-optimizer.hh>
# include <hpp/core/path-vector.hh> #include <hpp/core/problem-solver.hh> // PathOptimizerBuilder_t
# include <hpp/core/path-optimizer.hh> #include <hpp/manipulation/config.hh>
# include <hpp/core/problem.hh> #include <hpp/manipulation/fwd.hh>
# include <hpp/core/problem-solver.hh> #include <hpp/manipulation/graph/fwd.hh>
# include <hpp/manipulation/fwd.hh>
# include <hpp/manipulation/graph/fwd.hh>
# include <hpp/manipulation/config.hh>
# include <hpp/manipulation/constraint-set.hh>
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
using hpp::core::Path; using hpp::core::Path;
using hpp::core::PathPtr_t; using hpp::core::PathPtr_t;
using hpp::core::PathVector; using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t; using hpp::core::PathVectorPtr_t;
/// \addtogroup path_optimization /// \addtogroup path_optimization
/// \{ /// \{
/// Path optimizer for paths created with the constraint graph /// Path optimizer for paths created with the constraint graph
/// ///
/// This class encapsulates another path optimizer class. This optimizer /// This class encapsulates another path optimizer class. This optimizer
/// calls the inner optimizer on every subpaths with the same set of /// calls the inner optimizer on every subpaths with the same set of
/// constraints. /// constraints.
class HPP_MANIPULATION_DLLAPI GraphOptimizer : public PathOptimizer class HPP_MANIPULATION_DLLAPI GraphOptimizer : public PathOptimizer {
{ public:
public: typedef core::PathOptimizerBuilder_t PathOptimizerBuilder_t;
typedef core::PathOptimizerBuilder_t PathOptimizerBuilder_t;
template <typename TraitsOrInnerType>
template <typename TraitsOrInnerType> static GraphOptimizerPtr_t create(const core::ProblemConstPtr_t& problem);
static GraphOptimizerPtr_t create (const core::Problem& problem);
virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
/// Get the encapsulated optimizer
/// Get the encapsulated optimizer const PathOptimizerPtr_t& innerOptimizer() { return pathOptimizer_; }
const PathOptimizerPtr_t& innerOptimizer ()
{ protected:
return pathOptimizer_; /// Constructor
} GraphOptimizer(const core::ProblemConstPtr_t& problem,
PathOptimizerBuilder_t factory)
protected: : PathOptimizer(problem), factory_(factory), pathOptimizer_() {}
/// Constructor
GraphOptimizer (const core::Problem& problem, PathOptimizerBuilder_t factory) : private:
PathOptimizer (problem), factory_ (factory), pathOptimizer_ () PathOptimizerBuilder_t factory_;
{}
/// The encapsulated PathOptimizer
private: PathOptimizerPtr_t pathOptimizer_;
PathOptimizerBuilder_t factory_; };
/// \}
/// The encapsulated PathOptimizer
PathOptimizerPtr_t pathOptimizer_; /// Member function definition
}; template <typename TraitsOrInnerType>
/// \} GraphOptimizerPtr_t GraphOptimizer::create(
const core::ProblemConstPtr_t& problem) {
/// Member function definition return GraphOptimizerPtr_t(
template <typename TraitsOrInnerType> new GraphOptimizer(problem, TraitsOrInnerType::create));
GraphOptimizerPtr_t GraphOptimizer::create }
(const core::Problem& problem)
{ } // namespace manipulation
return GraphOptimizerPtr_t ( } // namespace hpp
new GraphOptimizer (problem, TraitsOrInnerType::create)
); #endif // HPP_MANIPULATION_GRAPHOPTIMIZER_HH
}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPHOPTIMIZER_HH
include/hpp/manipulation/graph-path-validation.hh
View file @ 539aab52
// Copyright (c) 2014, LAAS-CNRS // Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // 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 #ifndef HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
# define 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>
# include "hpp/manipulation/fwd.hh" #include <hpp/core/obstacle-user.hh>
# include "hpp/manipulation/graph/graph.hh" #include <hpp/core/path-validation.hh>
# include "hpp/manipulation/graph/node.hh" #include <hpp/manipulation/config.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/manipulation/graph/fwd.hh>
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
using hpp::core::PathValidation; using hpp::core::Path;
using hpp::core::PathValidationPtr_t; using hpp::core::PathPtr_t;
using hpp::core::Path; using hpp::core::PathValidation;
using hpp::core::PathPtr_t; using hpp::core::PathValidationPtr_t;
using hpp::core::PathVector; using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t; using hpp::core::PathVectorPtr_t;
using graph::GraphPtr_t;
/// \addtogroup validation
/// \addtogroup validation /// \{
/// \{
/// Path validation for a constraint graph
/// Path validation for a constraint graph ///
/// /// This class encapsulates another path validation class.
/// This class encapsulates another path validation class. /// The encapsulated path validation is responsible for collision
/// The encapsulated path validation is responsible for collision /// checking, whereas this class checks if a path is valid regarding
/// checking, whereas this class checks if a path is valid regarding /// the constraint graph.
/// the constraint graph. class HPP_MANIPULATION_DLLAPI GraphPathValidation
class HPP_MANIPULATION_DLLAPI GraphPathValidation : public PathValidation : public PathValidation,
{ public core::ObstacleUserInterface {
public: public:
/// Check that path is valid regarding the constraint graph. /// Check that path is valid regarding the constraint graph.
/// ///
/// \param path the path to check for validity, /// \param path the path to check for validity,
/// \param reverse if true check from the end, /// \param reverse if true check from the end,
/// \retval the extracted valid part of the path, pointer to path if /// \retval the extracted valid part of the path, pointer to path if
/// path is valid, /// path is valid,
/// \retval report information about the validation process. unused in /// \retval report information about the validation process. unused in
/// this case, /// this case,
/// \return whether the whole path is valid. /// \return whether the whole path is valid.
/// ///
/// \notice Call the encapsulated PathValidation::validate. /// \notice Call the encapsulated PathValidation::validate.
virtual bool validate (const PathPtr_t& path, bool reverse, virtual bool validate(const PathPtr_t& path, bool reverse,
PathPtr_t& validPart, PathPtr_t& validPart,
PathValidationReportPtr_t& report); PathValidationReportPtr_t& report);
/// Set the encapsulated path validator. /// Set the encapsulated path validator.
void innerValidation (const PathValidationPtr_t& pathValidation) void innerValidation(const PathValidationPtr_t& pathValidation) {
{ pathValidation_ = pathValidation;
pathValidation_ = pathValidation; }
}
/// Get the encapsulated path validator.
/// Get the encapsulated path validator. const PathValidationPtr_t& innerValidation() { return pathValidation_; }
const PathValidationPtr_t& innerValidation ()
{ /// Set the graph of constraints
return pathValidation_; void constraintGraph(const graph::GraphPtr_t& graph) {
} constraintGraph_ = graph;
}
/// Set the graph of constraints
void constraintGraph (const graph::GraphPtr_t& graph) /// Get the graph of constraints
{ graph::GraphPtr_t constraintGraph() const { return constraintGraph_; }
constraintGraph_ = graph;
} /// Create a new instance of this class.
/// \param pathValidation a PathValidation that is responsible for collision
/// Get the graph of constraints // checking.
graph::GraphPtr_t constraintGraph () const // \param graph A pointer to the constraint graph.
{ static GraphPathValidationPtr_t create(
return constraintGraph_; const PathValidationPtr_t& pathValidation);
}
template <typename T>
/// Create a new instance of this class. static GraphPathValidationPtr_t create(const pinocchio::DevicePtr_t& robot,
/// \param pathValidation a PathValidation that is responsible for collision const value_type& stepSize);
// checking.
// \param graph A pointer to the constraint graph. /// Add obstacle in the environment
static GraphPathValidationPtr_t create ( ///
const PathValidationPtr_t& pathValidation); /// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
template <typename T> /// hpp::core::ObstacleUserInterface::addObstacle in case of success.
static GraphPathValidationPtr_t create ( void addObstacle(const hpp::core::CollisionObjectConstPtr_t& object) {
const model::DevicePtr_t& robot, const value_type& stepSize); shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
void addObstacle (const hpp::core::CollisionObjectPtr_t&); if (oui) oui->addObstacle(object);
}
/// Remove a collision pair between a joint and an obstacle
/// \param joint the joint that holds the inner objects, /// Remove a collision pair between a joint and an obstacle
/// \param obstacle the obstacle to remove. ///
/// \notice collision configuration validation needs to know about /// Dynamic cast inner path validation into
/// obstacles. This virtual method does nothing for configuration /// hpp::core::ObstacleUserInterface and calls
/// validation methods that do not care about obstacles. /// hpp::core::ObstacleUserInterface::removeObstacleFromJoint in case
virtual void removeObstacleFromJoint (const JointPtr_t& joint, /// of success.
const model::CollisionObjectPtr_t& obstacle) void removeObstacleFromJoint(
{ const JointPtr_t& joint,
assert (pathValidation_); const pinocchio::CollisionObjectConstPtr_t& obstacle) {
pathValidation_->removeObstacleFromJoint (joint, obstacle); shared_ptr<core::ObstacleUserInterface> oui =
} HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->removeObstacleFromJoint(joint, obstacle);
protected: }
/// Constructor
GraphPathValidation (const PathValidationPtr_t& pathValidation); /// \brief Filter collision pairs.
///
private: /// Dynamic cast inner path validation into
/// Do validation regarding the constraint graph for PathVector /// hpp::core::ObstacleUserInterface and calls
bool impl_validate (const PathVectorPtr_t& path, bool reverse, /// hpp::core::ObstacleUserInterface::filterCollisionPairs in case of
PathPtr_t& validPart, PathValidationReportPtr_t& report); /// success.
/// Do validation regarding the constraint graph for Path void filterCollisionPairs(
bool impl_validate (const PathPtr_t& path, bool reverse, const core::RelativeMotion::matrix_type& relMotion) {
PathPtr_t& validPart, PathValidationReportPtr_t& report); shared_ptr<core::ObstacleUserInterface> oui =
/// The encapsulated PathValidation. HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
PathValidationPtr_t pathValidation_; if (oui) oui->filterCollisionPairs(relMotion);
/// Pointer to the constraint graph. }
GraphPtr_t constraintGraph_;
}; /// Set different security margins for collision pairs
///
template <typename T> /// Dynamic cast inner path validation into
GraphPathValidationPtr_t GraphPathValidation::create /// hpp::core::ObstacleUserInterface and calls
(const model::DevicePtr_t& robot, const value_type& stepSize) /// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
{ /// success.
return GraphPathValidation::create (T::create (robot, stepSize)); void setSecurityMargins(const matrix_t& securityMargins) {
} shared_ptr<core::ObstacleUserInterface> oui =
/// \} HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
} // namespace manipulation if (oui) oui->setSecurityMargins(securityMargins);
} // namespace hpp }
#endif // HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH /// \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 // Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 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 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// General Lesser Public License for more details. You should have // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// received a copy of the GNU Lesser General Public License along with // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. // 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 #ifndef HPP_MANIPULATION_GRAPH_DOT_HH
# define HPP_MANIPULATION_GRAPH_DOT_HH #define HPP_MANIPULATION_GRAPH_DOT_HH
# include <ostream> #include <list>
# include <sstream> #include <map>
# include <map> #include <ostream>
# include <list> #include <sstream>
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
namespace graph { namespace graph {
namespace dot { namespace dot {
struct DrawingAttributes { struct DrawingAttributes {
typedef std::pair <std::string, std::string> Pair; typedef std::pair<std::string, std::string> Pair;
typedef std::map <std::string, std::string> Map; typedef std::map<std::string, std::string> Map;
std::string separator, openSection, closeSection; std::string separator, openSection, closeSection;
Map attr; Map attr;
inline void insertWithQuote (const std::string& K, const std::string& V) { inline void insertWithQuote(const std::string& K, const std::string& V) {
attr.insert (Pair (K, "\"" + V + "\"")); attr.insert(Pair(K, "\"" + V + "\""));
} }
inline void insert (const std::string& K, const std::string& V) { inline void insert(const std::string& K, const std::string& V) {
attr.insert (Pair (K, V)); attr.insert(Pair(K, V));
} }
std::string& operator [] (const std::string& K) { std::string& operator[](const std::string& K) { return attr[K]; }
return attr [K]; DrawingAttributes()
} : separator(", "), openSection("["), closeSection("]"), attr() {};
DrawingAttributes () : };
separator (", "), openSection ("["), closeSection ("]"),
attr () {};
};
struct Tooltip { struct Tooltip {
static const std::string tooltipendl; static const std::string tooltipendl;
typedef std::list <std::string> TooltipLineVector; typedef std::list<std::string> TooltipLineVector;
TooltipLineVector v; TooltipLineVector v;
Tooltip () : v() {}; Tooltip() : v() {};
inline std::string toStr () const { inline std::string toStr() const {
std::stringstream ss; std::stringstream ss;
size_t i = v.size (); size_t i = v.size();
for (TooltipLineVector::const_iterator for (TooltipLineVector::const_iterator it = v.begin(); it != v.end();
it = v.begin (); it != v.end (); ++it ) { ++it) {
ss << *it; ss << *it;
i--; i--;
if (i > 0) ss << tooltipendl; if (i > 0) ss << tooltipendl;
} }
return ss.str (); return ss.str();
} }
inline void addLine (const std::string& l) { inline void addLine(const std::string& l) { v.push_back(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); std::ostream& operator<<(std::ostream& os, const DrawingAttributes& da);
} // namespace dot } // namespace dot
} // namespace graph } // namespace graph
} // namespace manipulation } // namespace manipulation
} // namespace hpp } // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_DOT_HH #endif // HPP_MANIPULATION_GRAPH_DOT_HH
include/hpp/manipulation/graph/edge.hh
View file @ 539aab52
This diff is collapsed.
include/hpp/manipulation/graph/fwd.hh
View file @ 539aab52
// Copyright (c) 2014, LAAS-CNRS // Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 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 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// General Lesser Public License for more details. You should have // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// received a copy of the GNU Lesser General Public License along with // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. // 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 #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/statistics/distribution.hh>
#include <hpp/util/pointer.hh>
#include <vector> #include <vector>
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
namespace graph { namespace graph {
HPP_PREDEF_CLASS (Graph); HPP_PREDEF_CLASS(Graph);
HPP_PREDEF_CLASS (Node); HPP_PREDEF_CLASS(Edge);
HPP_PREDEF_CLASS (Edge); HPP_PREDEF_CLASS(State);
HPP_PREDEF_CLASS (WaypointEdge); HPP_PREDEF_CLASS(WaypointEdge);
HPP_PREDEF_CLASS (LevelSetEdge); HPP_PREDEF_CLASS(LevelSetEdge);
HPP_PREDEF_CLASS (NodeSelector); HPP_PREDEF_CLASS(StateSelector);
HPP_PREDEF_CLASS (GuidedNodeSelector); HPP_PREDEF_CLASS(GraphComponent);
HPP_PREDEF_CLASS (GraphComponent); HPP_PREDEF_CLASS(GuidedStateSelector);
typedef boost::shared_ptr < Graph > GraphPtr_t; typedef shared_ptr<Graph> GraphPtr_t;
typedef boost::shared_ptr < Node > NodePtr_t; typedef shared_ptr<State> StatePtr_t;
typedef boost::shared_ptr < Edge > EdgePtr_t; typedef shared_ptr<Edge> EdgePtr_t;
typedef boost::shared_ptr < WaypointEdge > WaypointEdgePtr_t; typedef shared_ptr<WaypointEdge> WaypointEdgePtr_t;
typedef boost::shared_ptr < LevelSetEdge > LevelSetEdgePtr_t; typedef shared_ptr<LevelSetEdge> LevelSetEdgePtr_t;
typedef boost::shared_ptr < NodeSelector > NodeSelectorPtr_t; typedef shared_ptr<StateSelector> StateSelectorPtr_t;
typedef boost::shared_ptr < GuidedNodeSelector > GuidedNodeSelectorPtr_t; typedef shared_ptr<GuidedStateSelector> GuidedStateSelectorPtr_t;
typedef boost::shared_ptr < GraphComponent > GraphComponentPtr_t; typedef shared_ptr<GraphComponent> GraphComponentPtr_t;
typedef std::vector < NodePtr_t > Nodes_t; typedef std::vector<GraphComponentWkPtr_t> GraphComponents_t;
typedef std::vector < EdgePtr_t > Edges_t; typedef std::vector<StatePtr_t> States_t;
typedef ::hpp::statistics::DiscreteDistribution< EdgePtr_t >::Weight_t Weight_t; typedef std::vector<EdgePtr_t> Edges_t;
typedef ::hpp::statistics::DiscreteDistribution< EdgePtr_t > Neighbors_t; typedef ::hpp::statistics::DiscreteDistribution<EdgePtr_t>::Weight_t Weight_t;
typedef std::vector < NodeSelectorPtr_t > NodeSelectors_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; class Histogram;
typedef hpp::core::ConstraintPtr_t ConstraintPtr_t; class StateHistogram;
typedef hpp::core::LockedJoint LockedJoint; class LeafHistogram;
typedef hpp::core::LockedJointPtr_t LockedJointPtr_t; typedef shared_ptr<Histogram> HistogramPtr_t;
typedef hpp::core::ConfigProjector ConfigProjector; typedef shared_ptr<StateHistogram> StateHistogramPtr_t;
typedef hpp::core::ConfigProjectorPtr_t ConfigProjectorPtr_t; typedef shared_ptr<LeafHistogram> LeafHistogramPtr_t;
typedef hpp::core::Equality Equality; typedef std::list<HistogramPtr_t> Histograms_t;
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 Validation;
class NodeHistogram; typedef shared_ptr<Validation> ValidationPtr_t;
class LeafHistogram; } // namespace graph
typedef boost::shared_ptr <Histogram> HistogramPtr_t; } // namespace manipulation
typedef boost::shared_ptr <NodeHistogram> NodeHistogramPtr_t; } // namespace hpp
typedef boost::shared_ptr <LeafHistogram> LeafHistogramPtr_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 // Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 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 // 2. Redistributions in binary form must reproduce the above copyright
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // notice, this list of conditions and the following disclaimer in the
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // documentation and/or other materials provided with the distribution.
// General Lesser Public License for more details. You should have //
// received a copy of the GNU Lesser General Public License along with // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. // "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 #ifndef HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH
# define HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH #define HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH
# include <string> #include <ostream>
# include <ostream> #include <string>
# include <hpp/util/exception.hh>
# include "hpp/manipulation/config.hh" #include "hpp/manipulation/config.hh"
# include "hpp/manipulation/deprecated.hh" #include "hpp/manipulation/deprecated.hh"
# include "hpp/manipulation/fwd.hh" #include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh" #include "hpp/manipulation/graph/dot.hh"
# include "hpp/manipulation/graph/dot.hh" #include "hpp/manipulation/graph/fwd.hh"
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
namespace graph { typedef constraints::ImplicitPtr_t ImplicitPtr_t;
HPP_MAKE_EXCEPTION ( HPP_MANIPULATION_DLLAPI, Bad_function_call ); namespace graph {
/// \defgroup constraint_graph Constraint Graph
/// \defgroup constraint_graph Constraint Graph
/// \addtogroup constraint_graph
/// \addtogroup constraint_graph /// \{
/// \{
/// Define common methods of the graph components.
/// Define common methods of the graph components. class HPP_MANIPULATION_DLLAPI GraphComponent {
class HPP_MANIPULATION_DLLAPI GraphComponent public:
{ virtual ~GraphComponent() {};
public:
/// Get the component by its ID. The validity of the GraphComponent /// Get the component name.
/// is not checked. const std::string& name() const;
static GraphComponentWkPtr_t get(std::size_t id);
/// Return the component id.
/// The list of elements const std::size_t& id() const { return id_; }
static const std::vector < GraphComponentWkPtr_t >& components ();
/// Add constraint to the component.
/// Get the component name. virtual void addNumericalConstraint(const ImplicitPtr_t& numConstraint);
const std::string& name() const;
/// Add a cost function Implicit to the component.
/// Set the component name. virtual void addNumericalCost(const ImplicitPtr_t& numCost);
/// \deprecated The name given at construction cannot be modified.
void name(const std::string& name) HPP_MANIPULATION_DEPRECATED; /// Reset the numerical constraints stored in the component.
virtual void resetNumericalConstraints();
/// Return the component id.
const std::size_t& id () const /// Insert the numerical constraints in a ConfigProjector
{ /// \return true is at least one ImplicitPtr_t was inserted.
return id_; bool insertNumericalConstraints(ConfigProjectorPtr_t& proj) const;
}
/// Get a reference to the NumericalConstraints_t
/// Add core::NumericalConstraint to the component. const NumericalConstraints_t& numericalConstraints() const;
/// \param passiveDofs see ConfigProjector::addNumericalConstraint
// for more information. /// Get a reference to the NumericalConstraints_t
virtual void addNumericalConstraint ( const NumericalConstraints_t& numericalCosts() const;
const NumericalConstraintPtr_t& numConstraint,
const SizeIntervals_t& passiveDofs = SizeIntervals_t ()); /// Set the parent graph.
void parentGraph(const GraphWkPtr_t& parent);
/// Add core::DifferentiableFunction to the component.
virtual void addNumericalConstraint /// Set the parent graph.
(const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq) GraphPtr_t parentGraph() const;
HPP_MANIPULATION_DEPRECATED;
/// Print the component in DOT language.
/// Add core::LockedJoint constraint to the component. virtual std::ostream& dotPrint(
virtual void addLockedJointConstraint std::ostream& os,
(const LockedJointPtr_t& constraint); dot::DrawingAttributes da = dot::DrawingAttributes()) const;
/// Insert the numerical constraints in a ConfigProjector /// Invalidate the component
/// \return true is at least one NumericalConstraintPtr_t was inserted. /// The component needs to be initialized again.
bool insertNumericalConstraints (ConfigProjectorPtr_t& proj) const; virtual void invalidate() { isInit_ = false; }
/// Insert the LockedJoint constraints in a ConstraintSet /// Set whether hierachical constraints are solved level by level
/// \return true is at least one LockedJointPtr_t was inserted. /// \sa hpp::constraints::solver::HierarchicalIterative
bool insertLockedJoints (ConfigProjectorPtr_t& cs) const; void solveLevelByLevel(bool solveLevelByLevel) {
solveLevelByLevel_ = solveLevelByLevel;
/// Get a reference to the NumericalConstraints_t }
const NumericalConstraints_t& numericalConstraints() const;
/// Get whether hierachical constraints are solved level by level
/// Get a reference to the NumericalConstraints_t /// \sa hpp::constraints::solver::HierarchicalIterative
const IntervalsContainer_t& passiveDofs() const; bool solveLevelByLevel() const { return solveLevelByLevel_; }
/// Get a reference to the LockedJoints_t protected:
const LockedJoints_t& lockedJoints () const; /// Initialize the component
void init(const GraphComponentWkPtr_t& weak);
/// Set the parent graph.
void parentGraph(const GraphWkPtr_t& parent); GraphComponent(const std::string& name)
: isInit_(false), name_(name), id_(-1), solveLevelByLevel_(false) {}
/// Set the parent graph.
GraphPtr_t parentGraph() const; /// Stores the numerical constraints.
NumericalConstraints_t numericalConstraints_;
/// Print the component in DOT language. /// Stores the numerical costs.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const; NumericalConstraints_t numericalCosts_;
/// A weak pointer to the parent graph.
protected: GraphWkPtr_t graph_;
/// Initialize the component
void init (const GraphComponentWkPtr_t& weak); bool isInit_;
GraphComponent(const std::string& name) : name_ (name), id_(-1) void throwIfNotInitialized() const;
{}
/// Print the object in a stream.
/// Stores the numerical constraints. virtual std::ostream& print(std::ostream& os) const;
NumericalConstraints_t numericalConstraints_; friend std::ostream& operator<<(std::ostream&, const GraphComponent&);
/// Stores the passive dofs for each numerical constraints.
IntervalsContainer_t passiveDofs_; /// Populate DrawingAttributes tooltip
/// List of LockedJoint constraints virtual void populateTooltip(dot::Tooltip& tp) const;
LockedJoints_t lockedJoints_;
/// A weak pointer to the parent graph. virtual void initialize() = 0;
GraphWkPtr_t graph_;
private:
/// Print the object in a stream. /// Name of the component.
virtual std::ostream& print (std::ostream& os) const; std::string name_;
friend std::ostream& operator<< (std::ostream&, const GraphComponent&); /// Weak pointer to itself.
GraphComponentWkPtr_t wkPtr_;
/// Populate DrawingAttributes tooltip /// ID of the component (index in components vector).
virtual void populateTooltip (dot::Tooltip& tp) const; std::size_t id_;
/// Whether the constraints are solved level by level
private: bool solveLevelByLevel_;
/// Keep track of the created components in order to retrieve them friend class Graph;
/// easily. };
static std::vector < GraphComponentWkPtr_t > components_;
std::ostream& operator<<(std::ostream& os, const GraphComponent& graphComp);
/// Name of the component. /// \}
std::string name_; } // namespace graph
/// Weak pointer to itself. } // namespace manipulation
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 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 // Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 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 // 2. Redistributions in binary form must reproduce the above copyright
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // notice, this list of conditions and the following disclaimer in the
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // documentation and/or other materials provided with the distribution.
// General Lesser Public License for more details. You should have //
// received a copy of the GNU Lesser General Public License along with // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. // "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 #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/config.hh"
# include "hpp/manipulation/fwd.hh" #include "hpp/manipulation/constraint-set.hh"
# include "hpp/manipulation/graph/fwd.hh" #include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/graph-component.hh" #include "hpp/manipulation/graph/fwd.hh"
#include "hpp/manipulation/graph/graph-component.hh"
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
namespace graph { typedef constraints::ImplicitPtr_t ImplicitPtr_t;
/// \addtogroup constraint_graph namespace graph {
/// \{ /// \addtogroup constraint_graph
/// \{
/// Description of the constraint graph.
/// /// Description of the constraint graph.
/// This class contains a graph representing a robot with several ///
/// end-effectors. /// This class contains a graph representing a a manipulation problem
/// ///
/// One must make sure not to create loop with shared pointers. /// One must make sure not to create loop with shared pointers.
/// To ensure that, the classes are defined as follow: /// To ensure that, the classes are defined as follow:
/// - A Graph owns (i.e. has a shared pointer to) the NodeSelector s /// - A Graph owns (i.e. has a shared pointer to) the StateSelector s
/// - A NodeSelector owns the Node s related to one gripper. /// - A StateSelector owns the Node s related to one gripper.
/// - A Node owns its outgoing Edge s. /// - A State owns its outgoing Edge s.
/// - An Edge does not own anything. /// - An Edge does not own anything.
class HPP_MANIPULATION_DLLAPI Graph : public GraphComponent ///
{ /// \note The graph and all its components have a unique index starting
public: /// at 0 for the graph itself. The index of a component can be retrieved
/// Create a new Graph. /// using method GraphComponent::id.
/// class HPP_MANIPULATION_DLLAPI Graph : public GraphComponent {
/// \param robot a manipulation robot public:
/// \param problem a pointer to the problem /// Create a new Graph.
static GraphPtr_t create(const std::string& name, DevicePtr_t robot, ///
const ProblemPtr_t& problem); /// \param robot a manipulation robot
/// \param problem a pointer to the problem
/// Create and insert a NodeSelector inside the graph. static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
NodeSelectorPtr_t createNodeSelector (const std::string& name); const ProblemPtr_t& problem);
/// Set the nodeSelector GraphPtr_t self() const { return wkPtr_.lock(); }
/// \warning This should be done before adding nodes to the node
/// selector otherwise the pointer to the parent graph will NOT be /// Create and insert a state selector inside the graph.
/// valid. StateSelectorPtr_t createStateSelector(const std::string& name);
void nodeSelector (NodeSelectorPtr_t ns);
/// Set the state selector
/// Get the nodeSelector /// \warning This should be done before adding nodes to the node
NodeSelectorPtr_t nodeSelector () const /// selector otherwise the pointer to the parent graph will NOT be
{ /// valid.
return nodeSelector_; void stateSelector(StateSelectorPtr_t ns);
}
/// Get the state selector
/// Returns the states of a configuration. StateSelectorPtr_t stateSelector() const { return stateSelector_; }
NodePtr_t getNode (ConfigurationIn_t config) const;
/// Returns the state of a configuration.
/// Returns the state of a roadmap node StatePtr_t getState(ConfigurationIn_t config) const;
NodePtr_t getNode(RoadmapNodePtr_t node) const;
/// Returns the state of a roadmap node
/// Get possible edges between two nodes. StatePtr_t getState(RoadmapNodePtr_t node) const;
Edges_t getEdges (const NodePtr_t& from, const NodePtr_t& to) const;
/// Get possible edges between two nodes.
/// Select randomly outgoing edge of the given node. Edges_t getEdges(const StatePtr_t& from, const StatePtr_t& to) const;
EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
/// Select randomly outgoing edge of the given node.
/// Constraint to project onto the Node. EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
/// \param the Node_t on which to project.
/// \return The initialized projector. /// Clear the vector of constraints and complements
ConstraintSetPtr_t configConstraint (const NodePtr_t& node); /// \sa registerConstraints
void clearConstraintsAndComplement();
/// Constraint to project onto the same leaf as config.
/// \param edges a list of edges defining the foliation. /// Register a triple of constraints to be inserted in nodes and edges
/// \return The constraint. /// \param constraint a constraint (grasp of placement)
ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge); /// \param complement the complement constraint
/// \param both combination of the constraint and its complement. Both
/// Get error of a config with respect to a node constraint /// constraints together corresponds to a full relative
/// /// transformation constraint
/// \param config Configuration, /// When inserting constraints in transitions of the graph,
/// \param node node containing the constraint to check config against /// in many cases, a constraint is associated to a state and
/// \retval error the error of the node constraint for the /// the complement constraint is associated to the
/// configuration /// transition itself. Registering those constraints
/// \return whether the configuration belongs to the node. /// priorly to graph construction makes possible to replace
/// Call method core::ConstraintSet::isSatisfied for the node /// the constraint and its complement by the combination of
/// constraints. /// both that is an explicit constraint.
bool getConfigErrorForNode (ConfigurationIn_t config, void registerConstraints(const ImplicitPtr_t& constraint,
const NodePtr_t& node, vector_t& error); const ImplicitPtr_t& complement,
const ImplicitPtr_t& both);
/// Constraint to project a path.
/// \param edge a list of edges defining the foliation. /// Test whether two constraints are complement of one another
/// \return The constraint. ///
ConstraintSetPtr_t pathConstraint (const EdgePtr_t& edge); /// \param constraint, complement two constraints to test
/// \retval combinationOfBoth constraint corresponding to combining
/// Set maximal number of iterations /// constraint and complement if result is true,
void maxIterations (size_type iterations); /// unchanged otherwise.
/// \return whether complement is the complement of constraint.
/// Get maximal number of iterations in config projector /// Two constraints are complement of one another if and only if
size_type maxIterations () const; /// combined they constitute a complement relative transformation
/// constraint. \sa Graph::registerConstraints
/// Set error threshold /// \warning argument order matters.
void errorThreshold (const value_type& threshold); bool isComplement(const ImplicitPtr_t& constraint,
const ImplicitPtr_t& complement,
/// Get error threshold in config projector ImplicitPtr_t& combinationOfBoth) const;
value_type errorThreshold () const;
/// Return the vector of tuples as registered in registerConstraints
/// Get the robot. /// \return a vector of tuples (c, c/complement, c/both) each of them
const DevicePtr_t& robot () const; /// corresponding to a constraint, the complement constraint
/// and the combination of boths.
/// Get the steering Method const ConstraintsAndComplements_t& constraintsAndComplements() const;
const ProblemPtr_t& problem () const;
/// Constraint to project onto the Node.
/// Register an histogram representing a foliation /// \param state the state on which to project.
void insertHistogram (const graph::HistogramPtr_t& hist) /// \return The initialized projector.
{ ConstraintSetPtr_t configConstraint(const StatePtr_t& state) const;
hists_.push_back (hist);
} /// Constraints a configuration in target state should satisfy
/// \param edge a transition
/// Get the histograms /// \return The set of constraints a configuration lying in the
std::list<HistogramPtr_t>& histograms () /// target state of the edge should satisfy. This set
{ /// includes the paths constraints of the edge.
return hists_; /// \sa Edge::targetConstraint.
} ConstraintSetPtr_t targetConstraint(const EdgePtr_t& edge) const;
/// Print the component in DOT language. /// Get error of a config with respect to a state constraint
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const; ///
/// \param config Configuration,
protected: /// \param state state containing the constraint to check config against
/// Initialization of the object. /// \retval error the error of the state constraint for the
void init (const GraphWkPtr_t& weak, DevicePtr_t robot); /// configuration
/// \return whether the configuration belongs to the state.
/// Constructor /// Call method core::ConstraintSet::isSatisfied for the state
/// \param sm a steering method to create paths from edges /// constraints.
Graph (const std::string& name, const ProblemPtr_t& problem) : bool getConfigErrorForState(ConfigurationIn_t config, const StatePtr_t& state,
GraphComponent (name), problem_ (problem) vector_t& error) const;
{}
/// Get error of a config with respect to an edge constraint
/// Print the object in a stream. ///
std::ostream& print (std::ostream& os) const; /// \param config Configuration,
/// \param edge edge containing the constraint to check config against
private: /// \retval error the error of the edge constraint for the
/// This list contains a node selector for each end-effector. /// configuration
NodeSelectorPtr_t nodeSelector_; /// \return whether the configuration can be a start point of a path
// of the edge
/// A set of constraints that will always be used, for example /// Call core::ConfigProjector::rightHandSideFromConfig with
/// stability constraints. /// input configuration and method core::ConstraintSet::isSatisfied
ConstraintPtr_t constraints_; /// for the edge constraint.
bool getConfigErrorForEdge(ConfigurationIn_t config, const EdgePtr_t& edge,
/// Keep a pointer to the composite robot. vector_t& error) const;
DevicePtr_t robot_;
/// Get error of a config with respect to an edge foliation leaf
/// Weak pointer to itself. ///
GraphWkPtr_t wkPtr_; /// \param leafConfig Configuration that determines the foliation leaf
/// \param config Configuration the error of which is computed
/// Map of constraint sets (from Node). /// \retval error the error
typedef std::map < NodePtr_t, ConstraintSetPtr_t > MapFromNode; /// \return whether config can be the end point of a path of the edge
typedef std::pair < NodePtr_t, ConstraintSetPtr_t > PairNodeConstraints; /// starting at leafConfig
MapFromNode constraintSetMapFromNode_; /// Call methods core::ConfigProjector::rightHandSideFromConfig with
/// leafConfig and then core::ConstraintSet::isSatisfied with config.
/// List of histograms /// on the edge constraints.
std::list<HistogramPtr_t> hists_; bool getConfigErrorForEdgeLeaf(ConfigurationIn_t leafConfig,
ConfigurationIn_t config,
/// Map of constraint sets (from Edge). const EdgePtr_t& edge, vector_t& error) const;
typedef std::map < EdgePtr_t, ConstraintSetPtr_t > MapFromEdge;
typedef std::pair < EdgePtr_t, ConstraintSetPtr_t > PairEdgeConstraints; /// Get error of a config with respect to the target of an edge foliation leaf
MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_; ///
ProblemPtr_t problem_; /// \param leafConfig Configuration that determines the foliation leaf
value_type errorThreshold_; /// \param config Configuration the error of which is computed
size_type maxIterations_; /// \retval error the error
}; // Class Graph /// \return whether config can be the end point of a path of the edge
/// starting at leafConfig
/// \} /// Call methods core::ConfigProjector::rightHandSideFromConfig with
} // namespace graph /// leafConfig and then core::ConstraintSet::isSatisfied with config.
} // namespace manipulation /// on the edge constraints.
bool getConfigErrorForEdgeTarget(ConfigurationIn_t leafConfig,
} // namespace hpp ConfigurationIn_t config,
const EdgePtr_t& edge,
#endif // HPP_MANIPULATION_GRAPH_GRAPH_HH 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 // Copyright (c) 2016, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 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 // 2. Redistributions in binary form must reproduce the above copyright
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // notice, this list of conditions and the following disclaimer in the
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // documentation and/or other materials provided with the distribution.
// General Lesser Public License for more details. You should have //
// received a copy of the GNU Lesser General Public License along with // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. // "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 #ifndef HPP_MANIPULATION_GRAPH_HELPER_HH
# define HPP_MANIPULATION_GRAPH_HELPER_HH #define HPP_MANIPULATION_GRAPH_HELPER_HH
# include <string>
# include <algorithm>
# include <boost/tuple/tuple.hpp> #include <algorithm>
#include <string>
#include <tuple>
# include "hpp/manipulation/config.hh" #include "hpp/manipulation/config.hh"
# include "hpp/manipulation/fwd.hh" #include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh" #include "hpp/manipulation/graph/fwd.hh"
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
namespace graph { typedef constraints::ImplicitPtr_t ImplicitPtr_t;
namespace helper { namespace graph {
/// \defgroup helper Helpers to build the graph of constraints namespace helper {
/// \addtogroup helper /// \defgroup helper Helpers to build the graph of constraints
/// \{ /// \addtogroup helper
/// \{
struct NumericalConstraintsAndPassiveDofs {
NumericalConstraints_t nc; NumericalConstraints_t merge_nc(const NumericalConstraints_t& a,
IntervalsContainer_t pdof; const NumericalConstraints_t& b) {
NumericalConstraintsAndPassiveDofs merge NumericalConstraints_t nc;
(const NumericalConstraintsAndPassiveDofs& other) { nc.reserve(a.size() + b.size());
NumericalConstraintsAndPassiveDofs ret; std::copy(a.begin(), a.end(), nc.begin());
// ret.nc.reserve (nc.size() + other.nc.size()); std::copy(b.begin(), b.end(), nc.begin());
ret.pdof.reserve (pdof.size() + other.pdof.size()); return nc;
}
std::copy (nc.begin(), nc.end(), ret.nc.begin());
std::copy (other.nc.begin(), other.nc.end(), ret.nc.begin()); struct FoliatedManifold {
// Manifold definition
std::copy (pdof.begin(), pdof.end(), ret.pdof.begin()); NumericalConstraints_t nc;
std::copy (other.pdof.begin(), other.pdof.end(), ret.pdof.begin()); LockedJoints_t lj;
return ret; NumericalConstraints_t nc_path;
} // Foliation definition
NumericalConstraints_t nc_fol;
template <bool forPath> void addToComp (GraphComponentPtr_t comp) const; LockedJoints_t lj_fol;
template <bool param> void specifyFoliation (LevelSetEdgePtr_t lse) const; FoliatedManifold merge(const FoliatedManifold& other) {
}; FoliatedManifold both;
both.nc = merge_nc(nc, other.nc);
struct FoliatedManifold { both.nc_path = merge_nc(nc_path, other.nc_path);
// Manifold definition
NumericalConstraintsAndPassiveDofs nc; std::copy(lj.begin(), lj.end(), both.lj.end());
LockedJoints_t lj; std::copy(other.lj.begin(), other.lj.end(), both.lj.end());
NumericalConstraintsAndPassiveDofs nc_path; return both;
// Foliation definition }
NumericalConstraintsAndPassiveDofs nc_fol;
LockedJoints_t lj_fol; void addToState(StatePtr_t comp) const;
void addToEdge(EdgePtr_t comp) const;
FoliatedManifold merge (const FoliatedManifold& other) { void specifyFoliation(LevelSetEdgePtr_t lse) const;
FoliatedManifold both;
both.nc = nc.merge (other.nc); bool foliated() const { return !lj_fol.empty() || !nc_fol.empty(); }
both.nc_path = nc_path.merge (other.nc_path); bool empty() const { return lj.empty() && nc.empty(); }
};
std::copy (lj.begin (), lj.end (), both.lj.end ());
std::copy (other.lj.begin (), other.lj.end (), both.lj.end ()); enum GraspingCase {
return both; NoGrasp = 1 << 0,
} GraspOnly = 1 << 1,
WithPreGrasp = 1 << 2
void addToNode (NodePtr_t comp) const; };
void addToEdge (EdgePtr_t comp) const; enum PlacementCase {
void specifyFoliation (LevelSetEdgePtr_t lse) const; NoPlace = 1 << 3,
PlaceOnly = 1 << 4,
bool foliated () const { WithPrePlace = 1 << 5
return !lj_fol.empty () || !nc_fol.nc.empty (); };
}
bool empty () const { struct Rule {
return lj.empty () && nc.nc.empty (); std::vector<std::string> grippers_;
} std::vector<std::string> handles_;
}; bool link_;
Rule() : grippers_(), handles_(), link_(false) {}
enum GraspingCase { };
NoGrasp = 0,
GraspOnly = 1 << 0, typedef std::vector<Rule> Rules_t;
WithPreGrasp = 1 << 1
}; /// Create edges according to the case.
enum PlacementCase { /// gCase is a logical OR combination of GraspingCase and PlacementCase
NoPlace = 1 << 2, ///
PlaceOnly = 1 << 3, /// When an argument is not relevant, use the default constructor
WithPrePlace = 1 << 4 /// of FoliatedManifold
}; template <int gCase>
Edges_t createEdges(
/// Create edges according to the case. const std::string& forwName, const std::string& backName,
/// gCase is a logical OR combination of GraspingCase and PlacementCase const StatePtr_t& from, const StatePtr_t& to, const size_type& wForw,
/// const size_type& wBack, const FoliatedManifold& grasp,
/// When an argument is not relevant, use the default constructor const FoliatedManifold& pregrasp, const FoliatedManifold& place,
/// of FoliatedManifold const FoliatedManifold& preplace, const bool levelSetGrasp,
template < int gCase > const bool levelSetPlace,
Edges_t createEdges ( const FoliatedManifold& submanifoldDef = FoliatedManifold());
const std::string& forwName, const std::string& backName,
const NodePtr_t& from, const NodePtr_t& to, EdgePtr_t createLoopEdge(
const size_type& wForw, const size_type& wBack, const std::string& loopName, const StatePtr_t& state, const size_type& w,
const FoliatedManifold& grasp, const FoliatedManifold& pregrasp, const bool levelSet,
const FoliatedManifold& place, const FoliatedManifold& preplace, const FoliatedManifold& submanifoldDef = FoliatedManifold());
const bool levelSetGrasp, const bool levelSetPlace,
const FoliatedManifold& submanifoldDef = FoliatedManifold () /// Create a waypoint edge taking into account:
); /// \li grasp
/// \li placement
EdgePtr_t createLoopEdge ( /// \li preplacement
const std::string& loopName,
const NodePtr_t& node, /// Create a waypoint edge taking into account
const size_type& w, /// \li grasp
const bool levelSet, /// \li pregrasp
const FoliatedManifold& submanifoldDef = FoliatedManifold () /// \li placement
);
void graspManifold(const GripperPtr_t& gripper, const HandlePtr_t& handle,
/// Create a waypoint edge taking into account: FoliatedManifold& grasp, FoliatedManifold& pregrasp);
/// \li grasp
/// \li placement /// The placement foliation constraint is built using
/// \li preplacement /// hpp::constraints::ConvexShapeMatcherComplement
void strictPlacementManifold(const ImplicitPtr_t placement,
/// Create a waypoint edge taking into account const ImplicitPtr_t preplacement,
/// \li grasp const ImplicitPtr_t placementComplement,
/// \li pregrasp FoliatedManifold& place,
/// \li placement FoliatedManifold& preplace);
/// \todo when the handle is a free flying object, add the robot DOFs /// The placement foliation constraint is built locked joints
/// as passive dofs to the numerical constraints for paths /// It is faster than strictPlacementManifold but the foliation
void graspManifold ( /// parametrisation is redundant.
const GripperPtr_t& gripper, const HandlePtr_t& handle, void relaxedPlacementManifold(const ImplicitPtr_t placement,
FoliatedManifold& grasp, FoliatedManifold& pregrasp); const ImplicitPtr_t preplacement,
const LockedJoints_t objectLocks,
/// The placement foliation constraint is built using FoliatedManifold& place,
/// hpp::constraints::ConvexShapeMatcherComplement FoliatedManifold& preplace);
void strictPlacementManifold (
const NumericalConstraintPtr_t placement, typedef std::tuple<ImplicitPtr_t, ImplicitPtr_t, LockedJoints_t>
const NumericalConstraintPtr_t preplacement, PlacementConstraint_t;
const NumericalConstraintPtr_t placementComplement, typedef std::vector<HandlePtr_t> Handles_t;
FoliatedManifold& place, FoliatedManifold& preplace); typedef std::vector<GripperPtr_t> Grippers_t;
/// Tuple representing an object as follows:
/// The placement foliation constraint is built locked joints /// \li PlacementConstraint_t constraint to place the object
/// It is faster than strictPlacementManifold but the foliation /// \li Handles_t list of handles of the object
/// parametrisation is redundant. /// \li std::size_t the index of this tuple in Objects_t.
void relaxedPlacementManifold ( /// \note the index must be unique, as object equallity is checked using this
const NumericalConstraintPtr_t placement, /// index.
const NumericalConstraintPtr_t preplacement, typedef std::tuple<PlacementConstraint_t, Handles_t, std::size_t> Object_t;
const LockedJoints_t objectLocks, typedef std::vector<Object_t> Objects_t;
FoliatedManifold& place, FoliatedManifold& preplace);
/// Fill a Graph
typedef boost::tuple <NumericalConstraintPtr_t, ///
NumericalConstraintPtr_t, /// \note It is assumed that a gripper can grasp only one handle and each
LockedJoints_t> /// handle cannot be grasped by several grippers at the same time.
PlacementConstraint_t; ///
typedef std::vector <HandlePtr_t> Handles_t; /// \param[in,out] graph must be an initialized empty Graph.
typedef std::vector <GripperPtr_t> Grippers_t; void graphBuilder(const ProblemSolverPtr_t& ps, const Objects_t& objects,
/// Tuple representing an object as follows: const Grippers_t& grippers, GraphPtr_t graph,
/// \li PlacementConstraint_t constraint to place the object const Rules_t& rules = Rules_t());
/// \li Handles_t list of handles of the object
/// \li std::size_t the index of this tuple in Objects_t. struct ObjectDef_t {
/// \note the index must be unique, as object equallity is checked using this index. std::string name;
typedef boost::tuple <PlacementConstraint_t, Handles_t, std::size_t> Object_t; Strings_t handles, shapes;
typedef std::vector <Object_t> Objects_t; };
/// Fill a Graph GraphPtr_t graphBuilder(const ProblemSolverPtr_t& ps,
/// const std::string& graphName, const Strings_t& griNames,
/// \note It is assumed that a gripper can grasp only one handle and each const std::vector<ObjectDef_t>& objs,
/// handle cannot be grasped by several grippers at the same time. const Strings_t& envNames, const Rules_t& rules,
/// const value_type& prePlaceWidth = 0.05);
/// \param[in,out] graph must be an initialized empty Graph. /// \}
void graphBuilder ( } // namespace helper
const Objects_t& objects, } // namespace graph
const Grippers_t& grippers, } // namespace manipulation
GraphPtr_t graph); } // namespace hpp
struct ObjectDef_t { #endif // HPP_MANIPULATION_GRAPH_HELPER_HH
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
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
This diff is collapsed.
include/hpp/manipulation/graph/state-selector.hh 0 → 100644
View file @ 539aab52
This diff is collapsed.
include/hpp/manipulation/graph/state.hh 0 → 100644
View file @ 539aab52
This diff is collapsed.
include/hpp/manipulation/graph/validation.hh 0 → 100644
View file @ 539aab52
This diff is collapsed.