Skip to content
Snippets Groups Projects

Compare revisions

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

Source

Select target project
No results found
Select Git revision

Target

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