RobotDebug.h

Go to the documentation of this file.

#pragma once
 
#include "Multibody.h"
#include "Components.h"
#include "../core/DebugScene.h"
 
namespace candlewick::multibody {

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

  void update(DebugScene &scene) {
    updateFrames(scene.registry());
    updateFrameVelocities(scene.registry());
  }
 
private:
  void updateFrames(entt::registry &reg);
  void updateFrameVelocities(entt::registry &reg);
 
  const pin::Model &m_robotModel;
  const pin::Data &m_robotData;
};
 
} // namespace candlewick::multibody