device.hh

Go to the documentation of this file.

//
// Copyright (c) 2016 CNRS
// Author: NMansard from Florent Lamiraux
//
//
// This file is part of hpp-pinocchio
// hpp-pinocchio 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-pinocchio 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-pinocchio  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_PINOCCHIO_DEVICE_HH
#define HPP_PINOCCHIO_DEVICE_HH

# include <iostream>
# include <vector>
# include <list>

# include <boost/thread/mutex.hpp>
# include <boost/thread/condition_variable.hpp>

# include <hpp/util/debug.hh>

# include <hpp/pinocchio/fwd.hh>
# include <hpp/pinocchio/frame.hh>
# include <hpp/pinocchio/pool.hh>
# include <hpp/pinocchio/config.hh>
# include <hpp/pinocchio/deprecated.hh>
# include <hpp/pinocchio/extra-config-space.hh>
# include <hpp/pinocchio/device-data.hh>
# include <hpp/pinocchio/device-sync.hh>

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()); }
 
      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();

      // -----------------------------------------------------------------------

      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;

      fcl::AABB computeAABB() const;

      void controlComputation (const Computation_t& flag);

    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_;
    }; // class Device

    inline std::ostream& operator<< (std::ostream& os, const hpp::pinocchio::Device& device)
    { return device.print(os); }

  } // namespace pinocchio
} // namespace hpp

#endif // HPP_PINOCCHIO_DEVICE_HH