Program Listing for File base-visualizer.hpp

↰ Return to documentation for file (include/pinocchio/visualizers/base-visualizer.hpp)

//
// Copyright (c) 2024-2025 INRIA
//

#ifndef __pinocchio_extra_base_visualizer_hpp__
#define __pinocchio_extra_base_visualizer_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/visualizers/config.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/geometry.hpp"

#include <boost/optional.hpp>
#include <utility>

namespace pinocchio
{
  namespace visualizers
  {
    typedef PINOCCHIO_SCALAR_TYPE_DEFAULT Scalar;
    PINOCCHIO_COMMON_TYPEDEF(PINOCCHIO_SCALAR_TYPE_DEFAULT, PINOCCHIO_OPTIONS_DEFAULT)

    typedef Eigen::Ref<const VectorXs> ConstVectorRef;
    typedef Eigen::Ref<const MatrixXs> ConstMatrixRef;

    class PINOCCHIO_VISUALIZERS_DLLAPI BaseVisualizer
    {
    public:
      typedef SE3::Matrix4 Matrix4;

      BaseVisualizer(
        const Model & model,
        const GeometryModel & visual_model,
        const GeometryModel * collision_model,
        Data & data,
        GeometryData & visual_data,
        GeometryData * collision_data);

      BaseVisualizer(
        const Model & model,
        const GeometryModel & visual_model,
        const GeometryModel * collision_model = nullptr);

      virtual ~BaseVisualizer();

      virtual void initViewer()
      {
      }

      virtual void loadViewerModel() = 0;

      virtual void rebuildData();

      virtual void display(const boost::optional<ConstVectorRef> & q = boost::none);

      template<typename D>
      void display(const Eigen::MatrixBase<D> & q)
      {
        boost::optional<ConstVectorRef> q_(q);
        display(q_);
      }

      virtual void play(const std::vector<ConstVectorRef> & qs, Scalar dt);

      void play(const ConstMatrixRef & qs, Scalar dt);

      virtual bool forceRedraw()
      {
        return true;
      }

      virtual void setCameraTarget(const Eigen::Ref<const Vector3> & /*target*/)
      {
      }

      virtual void setCameraPosition(const Eigen::Ref<const Vector3> & /*position*/)
      {
      }

      virtual void setCameraPose(const Eigen::Ref<const Matrix4> & /*pose*/)
      {
      }

      inline void setCameraPose(const SE3 & pose)
      {
        this->setCameraPose(pose.toHomogeneousMatrix());
      }

      virtual void setCameraZoom(Scalar /*value*/)
      {
      }

      virtual void enableCameraControl(bool)
      {
      }

      virtual void clean()
      {
      }

      const Model & model() const
      {
        return m_model;
      }

      const GeometryModel & visualModel() const
      {
        return *m_visualModel;
      }

      const GeometryModel & collisionModel() const
      {
        PINOCCHIO_THROW(
          hasCollisionModel(), std::logic_error, "No collision model in the visualizer.");
        return *m_collisionModel;
      }

      bool hasCollisionModel() const
      {
        return m_collisionModel != nullptr;
      }

      bool hasExternalData() const
      {
        return !m_ownedData;
      }

      Data & data()
      {
        return *m_data;
      }
      const Data & data() const
      {
        return *m_data;
      }

      GeometryData & visualData()
      {
        return *m_visualData;
      }
      const GeometryData & visualData() const
      {
        return *m_visualData;
      }

      GeometryData & collisionData()
      {
        PINOCCHIO_THROW(
          hasCollisionModel(), std::logic_error, "No collision model in the visualizer.");
        return *m_collisionData;
      }

      const GeometryData & collisionData() const
      {
        PINOCCHIO_THROW(
          hasCollisionModel(), std::logic_error, "No collision model in the visualizer.");
        return *m_collisionData;
      }

    protected:
      std::reference_wrapper<Model const> m_model;
      GeometryModel const * m_visualModel;
      GeometryModel const * m_collisionModel;

      Data * m_data;
      GeometryData * m_visualData;
      GeometryData * m_collisionData;
      bool m_ownedData;

      virtual void displayPrecall()
      {
      }
      virtual void displayImpl() = 0;

      void destroyData();
    };
  } // namespace visualizers
} // namespace pinocchio

#endif // ifndef __pinocchio_extra_base_visualizer_hxx__