From 72b4d756283af396f0a11b0c2592e0826f4972ba Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 19 Feb 2015 11:43:01 +0100
Subject: [PATCH] Add class Device and Container
---
CMakeLists.txt | 2 +
include/hpp/manipulation/container.hh | 84 ++++++++++++++
include/hpp/manipulation/device.hh | 107 ++++++++++++++++++
include/hpp/manipulation/fwd.hh | 12 +-
.../hpp/manipulation/graph-steering-method.hh | 2 +-
include/hpp/manipulation/problem-solver.hh | 10 +-
include/hpp/manipulation/robot.hh | 16 +--
src/graph-steering-method.cc | 2 +-
src/problem-solver.cc | 4 +-
src/robot.cc | 10 +-
10 files changed, 223 insertions(+), 26 deletions(-)
create mode 100644 include/hpp/manipulation/container.hh
create mode 100644 include/hpp/manipulation/device.hh
diff --git a/CMakeLists.txt b/CMakeLists.txt
index dc4b724..3edd7f4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,12 +49,14 @@ ENDIF ()
SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/fwd.hh
+ include/hpp/manipulation/container.hh
include/hpp/manipulation/axial-handle.hh
include/hpp/manipulation/handle.hh
include/hpp/manipulation/object.hh
include/hpp/manipulation/problem.hh
include/hpp/manipulation/problem-solver.hh
include/hpp/manipulation/robot.hh
+ include/hpp/manipulation/device.hh
include/hpp/manipulation/roadmap.hh
include/hpp/manipulation/manipulation-planner.hh
include/hpp/manipulation/graph-path-validation.hh
diff --git a/include/hpp/manipulation/container.hh b/include/hpp/manipulation/container.hh
new file mode 100644
index 0000000..0036d1c
--- /dev/null
+++ b/include/hpp/manipulation/container.hh
@@ -0,0 +1,84 @@
+// 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/>.
+
+#ifndef HPP_MANIPULATION_CONTAINER_HH
+# define HPP_MANIPULATION_CONTAINER_HH
+
+# include <map>
+# include <list>
+# include <boost/smart_ptr/shared_ptr.hpp>
+
+namespace hpp {
+ namespace manipulation {
+ template < typename Element, typename Key = std::string>
+ class HPP_MANIPULATION_DLLAPI Container
+ {
+ public:
+ typedef std::map <Key, Element> ElementMap_t;
+
+ /// Add an element
+ void add (const Key& name, const Element& element)
+ {
+ map_[name] = element;
+ }
+
+ /// Return the element named name
+ const Element& get (const Key& name) const
+ {
+ typename ElementMap_t::const_iterator it = map_.find (name);
+ if (it == map_.end ()) return Element ();
+ return it->second;
+ }
+
+ /// Return a list of all elements
+ /// \tparam ReturnType must have a push_back method.
+ template <typename ReturnType>
+ ReturnType getAllAs () const
+ {
+ ReturnType l;
+ for (typename ElementMap_t::const_iterator it = map_.begin ();
+ it != map_.end (); ++it)
+ l.push_back (it->second);
+ return l;
+ }
+
+ /// Return the underlying map.
+ const ElementMap_t& getAll () const
+ {
+ return map_;
+ }
+
+ /// Print object in a stream
+ std::ostream& print (std::ostream& os) const
+ {
+ for (typename ElementMap_t::const_iterator it = map_.begin ();
+ it != map_.end (); ++it) {
+ os << it->first << " : " << it->second << std::endl;
+ }
+ return os;
+ }
+
+ protected:
+ /// Constructor
+ Container () : map_ ()
+ {}
+
+ private:
+ ElementMap_t map_;
+ }; // class Container
+ } // namespace manipulation
+} // namespace hpp
+#endif // HPP_MANIPULATION_CONTAINER_HH
diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh
new file mode 100644
index 0000000..1f3febf
--- /dev/null
+++ b/include/hpp/manipulation/device.hh
@@ -0,0 +1,107 @@
+///
+/// Copyright (c) 2014 CNRS
+/// Authors: Florent Lamiraux
+///
+///
+// 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_DEVICE_HH
+# define HPP_MANIPULATION_DEVICE_HH
+
+# include <hpp/model/humanoid-robot.hh>
+# include <hpp/fcl/shape/geometric_shapes.h>
+
+# include "hpp/manipulation/fwd.hh"
+# include "hpp/manipulation/config.hh"
+# include "hpp/manipulation/container.hh"
+
+namespace hpp {
+ namespace manipulation {
+ /// Device with handles.
+ ///
+ /// As a deriving class of hpp::model::HumanoidRobot,
+ /// it is compatible with hpp::model::urdf::loadHumanoidRobot
+ class HPP_MANIPULATION_DLLAPI Device : public model::HumanoidRobot,
+ public Container <HandlePtr_t>, public Container <model::GripperPtr_t>,
+ public Container <TriangleList>
+ {
+ public:
+ typedef model::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;
+ }
+
+ /// Print object in a stream
+ virtual std::ostream& print (std::ostream& os) const
+ {
+ Parent_t::print (os);
+ // print handles
+ os << "Handles:" << std::endl;
+ Container <HandlePtr_t>::print (os);
+ // print grippers
+ os << "Grippers:" << std::endl;
+ Container <model::GripperPtr_t>::print (os);
+ return os;
+ }
+
+ /// Get an element of a container
+ template <typename Element>
+ const Element& get (const std::string& name) const
+ {
+ return Container <Element>::get (name);
+ }
+
+ /// Get the underlying map of a container
+ template <typename Element>
+ const typename Container<Element>::ElementMap_t& getAll () const
+ {
+ return Container <Element>::getAll ();
+ }
+
+ /// Add an element to a container
+ template <typename Element>
+ void add (const std::string& name, const Element& element)
+ {
+ Container <Element>::add (name, element);
+ }
+
+ 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;
+ }
+
+ private:
+ DeviceWkPtr_t self_;
+ }; // class Device
+ } // namespace manipulation
+} // namespace hpp
+#endif // HPP_MANIPULATION_DEVICE_HH
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 35e7289..08d1ba9 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -1,6 +1,6 @@
///
/// Copyright (c) 2014 CNRS
-/// Authors: Florent Lamiraux
+/// Authors: Florent Lamiraux, Joseph Mirabel
///
///
// This file is part of hpp-manipulation.
@@ -28,9 +28,13 @@ namespace fcl {
}
namespace hpp {
+ namespace model {
+ typedef boost::shared_ptr <const Device> DeviceConstPtr_t;
+ }
+
namespace manipulation {
- typedef model::Device Device;
- typedef model::DevicePtr_t DevicePtr_t;
+ 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;
@@ -78,7 +82,7 @@ namespace hpp {
typedef boost::shared_ptr < GraphSteeringMethod > GraphSteeringMethodPtr_t;
typedef core::PathProjectorPtr_t PathProjectorPtr_t;
- typedef std::vector <DevicePtr_t> Devices_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;
diff --git a/include/hpp/manipulation/graph-steering-method.hh b/include/hpp/manipulation/graph-steering-method.hh
index 9f68f58..afed8a5 100644
--- a/include/hpp/manipulation/graph-steering-method.hh
+++ b/include/hpp/manipulation/graph-steering-method.hh
@@ -38,7 +38,7 @@ namespace hpp {
{
public:
/// Constructor
- GraphSteeringMethod (const DevicePtr_t& robot);
+ GraphSteeringMethod (const model::DevicePtr_t& robot);
/// \name Graph of constraints applicable to the robot.
/// \{
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index e9ef9eb..3b56ce4 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -1,5 +1,5 @@
// Copyright (c) 2014 CNRS
-// Author: Florent Lamiraux
+// Author: Florent Lamiraux, Joseph Mirabel
//
// This file is part of hpp-manipulation-corba.
// hpp-manipulation-corba is free software: you can redistribute it
@@ -44,7 +44,7 @@ namespace hpp {
}
/// Set robot
/// Check that robot is of type hpp::manipulation::Robot
- virtual void robot (const DevicePtr_t& robot)
+ virtual void robot (const model::DevicePtr_t& robot)
{
robot_ = HPP_DYNAMIC_PTR_CAST (Robot, robot);
assert (robot_);
@@ -57,7 +57,7 @@ namespace hpp {
/// Add a single robot before building a composite robot
/// \param name key of the robot as stored in an internal map.
/// \param robot robot that is stored.
- void addRobot (const std::string& name, const DevicePtr_t& robot)
+ void addRobot (const std::string& name, const model::DevicePtr_t& robot)
{
robotsAndObjects_ [name] = robot;
}
@@ -79,7 +79,7 @@ namespace hpp {
/// Get robot with given name
///
/// throw if no robot is registered with this name.
- DevicePtr_t robot (const std::string& name) const;
+ model::DevicePtr_t robot (const std::string& name) const;
/// Get object with given name
///
@@ -176,7 +176,7 @@ namespace hpp {
void initializeProblem (ProblemPtr_t problem);
private:
- typedef std::map <const std::string, DevicePtr_t> RobotsandObjects_t;
+ typedef std::map <const std::string, model::DevicePtr_t> RobotsandObjects_t;
RobotPtr_t robot_;
/// The pointer should point to the same object as core::Problem.
ProblemPtr_t problem_;
diff --git a/include/hpp/manipulation/robot.hh b/include/hpp/manipulation/robot.hh
index db11438..8981047 100644
--- a/include/hpp/manipulation/robot.hh
+++ b/include/hpp/manipulation/robot.hh
@@ -32,10 +32,10 @@ namespace hpp {
/// As a deriving from class hpp::core::Device, objects of this class
/// store a kinematic chain that contains a copy of the robot and objects
/// given as arguments to the constructor.
- class HPP_MANIPULATION_DLLAPI Robot : public Device
+ class HPP_MANIPULATION_DLLAPI Robot : public model::Device
{
public:
- typedef Device parent_t;
+ typedef model::Device parent_t;
/// Constructor
/// \param name of the new instance,
/// \param robot Robots that manipulate objects,
@@ -48,14 +48,14 @@ namespace hpp {
/// \param robotOrObject robot or object,
/// \param fullConfig configuration of the full kinematic chain,
/// \return configuration of robot or object
- ConfigurationIn_t configuration (const DevicePtr_t& robotOrObject,
+ ConfigurationIn_t configuration (const model::DevicePtr_t& robotOrObject,
ConfigurationIn_t fullConfig);
/// Get velocity of robot or object
/// \param robotOrObject robot or object,
/// \param fullVelocity velocity of the full kinematic chain,
/// \return velocity of robot or object
- vectorIn_t velocity (const DevicePtr_t& robotOrObject,
+ vectorIn_t velocity (const model::DevicePtr_t& robotOrObject,
vectorIn_t fullVelocity);
/// Get robot manipulating objects
@@ -103,7 +103,7 @@ namespace hpp {
/// the kinematic chain of robots and objects.
void init (const RobotWkPtr_t& self);
private:
- typedef std::map <DevicePtr_t, size_type> RankMap_t;
+ typedef std::map <model::DevicePtr_t, size_type> RankMap_t;
typedef std::map <std::string, HandlePtr_t> Handles_t;
typedef std::map <std::string, GripperPtr_t> Grippers_t;
/// Build the kinematic chain composed of the robot and of the manipulated
@@ -119,12 +119,12 @@ namespace hpp {
void copyHandles (const ObjectConstPtr_t& object);
/// Copy Device including kinematic chain.
void copyDevice (const JointPtr_t& rootJoint,
- const DeviceConstPtr_t& device);
+ const model::DeviceConstPtr_t& device);
/// Copy grippers of the device into composite robot.
- void copyGrippers (const DeviceConstPtr_t& device);
+ void copyGrippers (const model::DeviceConstPtr_t& device);
/// Add Collision pairs between the robots
- void addCollisions(const DeviceConstPtr_t& device);
+ void addCollisions(const model::DeviceConstPtr_t& device);
Devices_t robots_;
/// Set of objects
diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc
index 93c7cba..bf8a9ef 100644
--- a/src/graph-steering-method.cc
+++ b/src/graph-steering-method.cc
@@ -23,7 +23,7 @@
namespace hpp {
namespace manipulation {
- GraphSteeringMethod::GraphSteeringMethod (const DevicePtr_t& robot) :
+ GraphSteeringMethod::GraphSteeringMethod (const model::DevicePtr_t& robot) :
SteeringMethod (), robot_ (robot),
distance_ (core::WeighedDistance::create (robot))
{
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index c70949b..74a2a7d 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -46,7 +46,7 @@ namespace hpp {
Names_t::const_iterator itName = robotNames.begin ();
for (;
itName != robotNames.end (); ++itName) {
- const DevicePtr_t& rob (robotsAndObjects_ [*itName]);
+ const model::DevicePtr_t& rob (robotsAndObjects_ [*itName]);
ObjectPtr_t object = HPP_DYNAMIC_PTR_CAST (Object, rob);
if (object) {
objects.push_back (object);
@@ -59,7 +59,7 @@ namespace hpp {
hppDout (info, *composite);
}
- DevicePtr_t ProblemSolver::robot (const std::string& name) const
+ model::DevicePtr_t ProblemSolver::robot (const std::string& name) const
{
RobotsandObjects_t::const_iterator it =
robotsAndObjects_.find (name);
diff --git a/src/robot.cc b/src/robot.cc
index fa38c79..3130c7c 100644
--- a/src/robot.cc
+++ b/src/robot.cc
@@ -126,14 +126,14 @@ namespace hpp {
}
void Robot::copyDevice(const JointPtr_t& rootJoint,
- const DeviceConstPtr_t& device)
+ const model::DeviceConstPtr_t& device)
{
copyKinematicChain (rootJoint, device->rootJoint ());
copyGrippers (device);
addCollisions(device);
}
- void Robot::copyGrippers (const DeviceConstPtr_t& device)
+ void Robot::copyGrippers (const model::DeviceConstPtr_t& device)
{
for (model::Grippers_t::const_iterator itGripper =
device->grippers ().begin ();
@@ -160,14 +160,14 @@ namespace hpp {
return shPtr;
}
- ConfigurationIn_t Robot::configuration (const DevicePtr_t& robotOrObject,
+ ConfigurationIn_t Robot::configuration (const model::DevicePtr_t& robotOrObject,
ConfigurationIn_t fullConfig)
{
return fullConfig.segment (rankInConfiguration_ [robotOrObject],
robotOrObject->configSize ());
}
- vectorIn_t Robot::velocity (const DevicePtr_t& robotOrObject,
+ vectorIn_t Robot::velocity (const model::DevicePtr_t& robotOrObject,
vectorIn_t fullVelocity)
{
return fullVelocity.segment (rankInVelocity_ [robotOrObject],
@@ -237,7 +237,7 @@ namespace hpp {
updateDistances ();
}
- void Robot::addCollisions (const DeviceConstPtr_t& device)
+ void Robot::addCollisions (const model::DeviceConstPtr_t& device)
{
JointVector_t joints = device->getJointVector ();
// Cycle through all joint pairs
--
GitLab