device.hh

Go to the documentation of this file.

//
// Copyright (c) 2016 CNRS
// Author: NMansard from Florent Lamiraux
//
//
 
// 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_PINOCCHIO_DEVICE_HH
#define HPP_PINOCCHIO_DEVICE_HH
 
#include <boost/thread/condition_variable.hpp>
#include <boost/thread/mutex.hpp>
#include <hpp/pinocchio/config.hh>
#include <hpp/pinocchio/deprecated.hh>
#include <hpp/pinocchio/device-data.hh>
#include <hpp/pinocchio/device-sync.hh>
#include <hpp/pinocchio/extra-config-space.hh>
#include <hpp/pinocchio/frame.hh>
#include <hpp/pinocchio/fwd.hh>
#include <hpp/pinocchio/pool.hh>
#include <hpp/util/debug.hh>
#include <hpp/util/serialization-fwd.hh>
#include <iostream>
#include <list>
#include <vector>
 
namespace hpp {
namespace pinocchio {
 
 
class HPP_PINOCCHIO_DLLAPI Device : public AbstractDevice {
  friend class Joint;
  friend class Frame;
  friend class DeviceSync;
  friend class CollisionObject;
  friend class CenterOfMassComputation;
 
 public:
  typedef std::pair<JointPtr_t, JointPtr_t> CollisionPair_t;
  typedef std::list<CollisionPair_t> CollisionPairs_t;
 
  // -----------------------------------------------------------------------
  virtual ~Device();
 
  virtual DevicePtr_t clone() const { return createCopy(weakPtr_.lock()); }
  DevicePtr_t cloneConst() const { return createCopyConst(weakPtr_.lock()); }
 
  DevicePtr_t self() const { return weakPtr_.lock(); }
 
  const std::string& name() const { return name_; }
 
  static DevicePtr_t create(const std::string& name);
 
  static DevicePtr_t createCopy(const DevicePtr_t& device);
  static DevicePtr_t createCopyConst(const DeviceConstPtr_t& device);
 
  // -----------------------------------------------------------------------
 
  void setModel(ModelPtr_t modelPtr) { model_ = modelPtr; }
 
  void setGeomModel(GeomModelPtr_t geomModelPtr) { geomModel_ = geomModelPtr; }
 
  void setData(DataPtr_t dataPtr) {
    d().data_ = dataPtr;
    resizeState();
  }
  void createData();
 
  void setGeomData(GeomDataPtr_t geomDataPtr) {
    d().geomData_ = geomDataPtr;
    resizeState();
  }
  void createGeomData();
 
  virtual void removeJoints(const std::vector<std::string>& jointNames,
                            Configuration_t referenceConfig);
 
  // -----------------------------------------------------------------------
 
  struct JointLinearConstraint {
    value_type offset, multiplier;
    JointPtr_t joint, reference;
  };
 
  JointPtr_t rootJoint() const;
 
  Frame rootFrame() const;
 
  size_type nbJoints() const;
 
  JointPtr_t jointAt(const size_type& i) const;
 
  JointPtr_t getJointAtConfigRank(const size_type& r) const;
 
  JointPtr_t getJointAtVelocityRank(const size_type& r) const;
 
  JointPtr_t getJointByName(const std::string& name) const;
 
  JointPtr_t getJointByBodyName(const std::string& name) const;
 
  Frame getFrameByName(const std::string& name) const;
 
  size_type configSize() const;
 
  size_type numberDof() const;
 
  const LiegroupSpacePtr_t& configSpace() const { return configSpace_; }
 
  const LiegroupSpacePtr_t& RnxSOnConfigSpace() const {
    return configSpaceRnxSOn_;
  }
 
  Configuration_t neutralConfiguration() const;
 
  void addJointConstraint(JointLinearConstraint constraint);
 
  const std::vector<JointLinearConstraint>& jointConstraints() const {
    return jointConstraints_;
  }
 
  // -----------------------------------------------------------------------
 
  ExtraConfigSpace& extraConfigSpace() { return extraConfigSpace_; }
 
  const ExtraConfigSpace& extraConfigSpace() const { return extraConfigSpace_; }
 
  virtual void setDimensionExtraConfigSpace(const size_type& dimension) {
    extraConfigSpace_.setDimension(dimension);
    resizeState();
  }
 
  // -----------------------------------------------------------------------
 
  void addGripper(const GripperPtr_t& gripper) { grippers_.push_back(gripper); }
 
  Grippers_t& grippers() { return grippers_; }
 
  const Grippers_t& grippers() const { return grippers_; }
 
  // -----------------------------------------------------------------------
 
  BodyPtr_t obstacles() const;
 
  size_type nbObjects() const;
 
  CollisionObjectPtr_t objectAt(const size_type& i) const;
 
  bool collisionTest(const bool stopAtFirstCollision = true);
 
  void computeDistances();
 
  const DistanceResults_t& distanceResults() const;
  // -----------------------------------------------------------------------
 
  void numberDeviceData(const size_type& s);
 
  size_type numberDeviceData() const;
 
  // -----------------------------------------------------------------------
 
  virtual std::ostream& print(std::ostream& os) const;
 
  coal::AABB computeAABB() const;
 
 protected:
  Device(const std::string& name);
 
  void init(const DeviceWkPtr_t& weakPtr);
  void initCopy(const DeviceWkPtr_t& weakPtr, const Device& other);
 
  Device(const Device& device);
 
 private:
  void resizeState();
 
 protected:
  DeviceData d_;
 
  DeviceData& d() { return d_; }
  DeviceData const& d() const { return d_; }
 
  inline void invalidate() { d_.invalidate(); }
 
  std::string name_;
  // Grippers
  Grippers_t grippers_;
  LiegroupSpacePtr_t configSpace_, configSpaceRnxSOn_;
  // Extra configuration space
  ExtraConfigSpace extraConfigSpace_;
  // Joint linear constraints
  std::vector<JointLinearConstraint> jointConstraints_;
  DeviceWkPtr_t weakPtr_;
 
 private:
  Pool<DeviceData> datas_;
 
 protected:
  Device() {}
 
 private:
  HPP_SERIALIZABLE_SPLIT();
};  // class Device
 
void replaceGeometryByConvexHull(GeomModel& geomModel,
                                 const std::vector<std::string>& geometryNames);
 
inline std::ostream& operator<<(std::ostream& os,
                                const hpp::pinocchio::Device& device) {
  return device.print(os);
}
 
}  // namespace pinocchio
}  // namespace hpp
 
BOOST_CLASS_EXPORT_KEY(hpp::pinocchio::Device)
 
#endif  // HPP_PINOCCHIO_DEVICE_HH