RobotDebug.h

Go to the documentation of this file.

#pragma once
 
#include "Multibody.h"
#include "../core/DebugScene.h"
#include <pinocchio/spatial/force.hpp>
 
namespace candlewick::multibody {

struct ExternalForceComponent {
  pin::FrameIndex frame_id;     //< Frame at which the force applies
  Forcef force{Forcef::Zero()}; //< Force value
  int lifetime = 1;             //< Arrow lifetime
  Float4 orig_color;
};

struct RobotDebugSystem final : IDebugSubSystem {
  inline static Float3 DEFAULT_TRIAD_SCALE = Float3::Constant(0.33333f);
  inline static float DEFAULT_VEL_SCALE = 0.5f;
 
  RobotDebugSystem(DebugScene &scene, const pin::Model &model,
                   const pin::Data &data)
      : IDebugSubSystem(scene), m_robotModel(&model), m_robotData(&data) {}
 
  entt::entity addFrameTriad(pin::FrameIndex frame_id,
                             const Float3 &scale = DEFAULT_TRIAD_SCALE);
 
  entt::entity addFrameVelocityArrow(pin::FrameIndex frame_id,
                                     float scale = DEFAULT_VEL_SCALE);

  void update() override;
 
  void destroyEntities();
 
  void renderDebugGui(const char *title);
 
  ~RobotDebugSystem() {
    this->destroyEntities();
    this->m_robotModel = nullptr;
    this->m_robotData = nullptr;
  }
 
  void reload(const pin::Model &model, const pin::Data &data) {
    this->destroyEntities();
    this->m_robotModel = &model;
    this->m_robotData = &data;
  }
 
private:
  pin::Model const *m_robotModel;
  pin::Data const *m_robotData;
};
 
} // namespace candlewick::multibody