|
| 1 | +// Copyright (c) 2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. |
| 2 | +#pragma once |
| 3 | + |
| 4 | +/** |
| 5 | + * @file loopback_state_interface.hpp |
| 6 | + * @brief RobotStateInterface that feeds commanded values back as measured state. |
| 7 | + * |
| 8 | + * This is intentionally minimal: every init*() succeeds, every getter returns a |
| 9 | + * sensible default (zeros / identity), and the set*() methods store the commanded |
| 10 | + * value so that it is returned the following cycle — modelling a perfect, |
| 11 | + * zero-delay actuator. |
| 12 | + */ |
| 13 | + |
| 14 | +#include <span> |
| 15 | +#include <string> |
| 16 | +#include <unordered_map> |
| 17 | +#include <vector> |
| 18 | +#include <cmath> |
| 19 | +#include <utility> |
| 20 | + |
| 21 | +#include "state_interface.hpp" |
| 22 | + |
| 23 | +namespace exploy::control::examples { |
| 24 | + |
| 25 | +/** |
| 26 | + * @brief Per-joint loopback state (position, velocity, effort). |
| 27 | + */ |
| 28 | +struct JointState { |
| 29 | + double position{0.0}; |
| 30 | + double velocity{0.0}; |
| 31 | + double effort{0.0}; |
| 32 | +}; |
| 33 | + |
| 34 | +/** |
| 35 | + * @brief Concrete RobotStateInterface with perfect loopback semantics. |
| 36 | + * |
| 37 | + * Commanded joint targets written via setJoint*() are returned by |
| 38 | + * jointPosition() / jointVelocity() / jointEffort() on the next query, |
| 39 | + * modelling a zero-latency, perfectly tracking actuator. |
| 40 | + * |
| 41 | + * All body/IMU/base state values are fixed at identity / zero so that an |
| 42 | + * ONNX policy can run without a physics simulation. |
| 43 | + */ |
| 44 | +class LoopbackRobotStateInterface : public RobotStateInterface { |
| 45 | + public: |
| 46 | + bool initBasePosW() override { return true; } |
| 47 | + bool initBaseQuatW() override { return true; } |
| 48 | + |
| 49 | + std::optional<Position> basePosW() const override { |
| 50 | + return Position{0.0, 0.0, 0.8}; |
| 51 | + } |
| 52 | + std::optional<Quaternion> baseQuatW() const override { |
| 53 | + return Quaternion::Identity(); |
| 54 | + } |
| 55 | + |
| 56 | + bool initBaseLinVelB() override { return true; } |
| 57 | + bool initBaseAngVelB() override { return true; } |
| 58 | + |
| 59 | + std::optional<LinearVelocity> baseLinVelB() const override { |
| 60 | + return LinearVelocity::Zero(); |
| 61 | + } |
| 62 | + std::optional<AngularVelocity> baseAngVelB() const override { |
| 63 | + return AngularVelocity::Zero(); |
| 64 | + } |
| 65 | + |
| 66 | + bool initJointPosition(const std::string& joint_name) override { |
| 67 | + joint_states_.emplace(joint_name, JointState{}); |
| 68 | + return true; |
| 69 | + } |
| 70 | + bool initJointVelocity(const std::string& joint_name) override { |
| 71 | + joint_states_.emplace(joint_name, JointState{}); |
| 72 | + return true; |
| 73 | + } |
| 74 | + bool initJointEffort(const std::string& joint_name) override { |
| 75 | + joint_states_.emplace(joint_name, JointState{}); |
| 76 | + return true; |
| 77 | + } |
| 78 | + bool initJointOutput(const std::string& /*joint_name*/) override { |
| 79 | + return true; |
| 80 | + } |
| 81 | + |
| 82 | + std::optional<double> jointPosition(const std::string& joint_name) const override { |
| 83 | + if (joint_states_.contains(joint_name)) { |
| 84 | + return joint_states_.at(joint_name).position; |
| 85 | + } |
| 86 | + return std::nullopt; |
| 87 | + } |
| 88 | + std::optional<double> jointVelocity(const std::string& joint_name) const override { |
| 89 | + if (joint_states_.contains(joint_name)) { |
| 90 | + return joint_states_.at(joint_name).velocity; |
| 91 | + } |
| 92 | + return std::nullopt; |
| 93 | + } |
| 94 | + std::optional<double> jointEffort(const std::string& joint_name) const override { |
| 95 | + if (joint_states_.contains(joint_name)) { |
| 96 | + return joint_states_.at(joint_name).effort; |
| 97 | + } |
| 98 | + return std::nullopt; |
| 99 | + } |
| 100 | + |
| 101 | + /// Loopback: store the commanded position so it is returned next cycle. |
| 102 | + bool setJointPosition(const std::string& joint_name, double position) override { |
| 103 | + joint_states_[joint_name].position = position; |
| 104 | + return true; |
| 105 | + } |
| 106 | + bool setJointVelocity(const std::string& joint_name, double velocity) override { |
| 107 | + joint_states_[joint_name].velocity = velocity; |
| 108 | + return true; |
| 109 | + } |
| 110 | + bool setJointEffort(const std::string& joint_name, double effort) override { |
| 111 | + joint_states_[joint_name].effort = effort; |
| 112 | + return true; |
| 113 | + } |
| 114 | + bool setJointPGain(const std::string& /*joint_name*/, double /*p_gain*/) override { |
| 115 | + return true; |
| 116 | + } |
| 117 | + bool setJointDGain(const std::string& /*joint_name*/, double /*d_gain*/) override { |
| 118 | + return true; |
| 119 | + } |
| 120 | + |
| 121 | + // Read-only access to the full joint state map (used by main.cpp for logging). |
| 122 | + const std::unordered_map<std::string, JointState>& jointStates() const { |
| 123 | + return joint_states_; |
| 124 | + } |
| 125 | + |
| 126 | + bool initSe2Velocity(const std::string& frame_name) override { |
| 127 | + se2_velocities_.emplace(frame_name, SE2Velocity::Zero()); |
| 128 | + return true; |
| 129 | + } |
| 130 | + bool setSe2Velocity(const std::string& frame_name, const SE2Velocity& velocity) override { |
| 131 | + se2_velocities_[frame_name] = velocity; |
| 132 | + return true; |
| 133 | + } |
| 134 | + |
| 135 | + // Read-only access to all SE(2) velocity outputs (used by main.cpp for logging). |
| 136 | + const std::unordered_map<std::string, SE2Velocity>& se2VelocityOutputs() const { |
| 137 | + return se2_velocities_; |
| 138 | + } |
| 139 | + |
| 140 | + bool initImuOrientationW(const std::string& /*imu_name*/) override { return true; } |
| 141 | + |
| 142 | + std::optional<Quaternion> imuOrientationW(const std::string& /*imu_name*/) const override { |
| 143 | + return Quaternion::Identity(); |
| 144 | + } |
| 145 | + |
| 146 | + bool initBodyPositionW(const std::string& /*body_name*/) override { return true; } |
| 147 | + bool initBodyOrientationW(const std::string& /*body_name*/) override { return true; } |
| 148 | + |
| 149 | + std::optional<Position> bodyPositionW(const std::string& /*body_name*/) const override { |
| 150 | + return Position{0.0, 0.0, 0.8}; |
| 151 | + } |
| 152 | + std::optional<Quaternion> bodyOrientationW(const std::string& /*body_name*/) const override { |
| 153 | + return Quaternion::Identity(); |
| 154 | + } |
| 155 | + |
| 156 | + bool initHeightScan(const std::string& sensor_name, |
| 157 | + const HeightScanConfig& config) override { |
| 158 | + // Pre-allocate zero-filled layers sized by the grid formula. |
| 159 | + const auto& p = config.pattern; |
| 160 | + const std::size_t nx = static_cast<std::size_t>(std::round(p.size.x() / p.resolution)) + 1; |
| 161 | + const std::size_t ny = static_cast<std::size_t>(std::round(p.size.y() / p.resolution)) + 1; |
| 162 | + const std::size_t n = nx * ny; |
| 163 | + |
| 164 | + HeightScan scan; |
| 165 | + for (const auto& layer : config.layer_names) { |
| 166 | + scan.layers[layer] = std::vector<double>(n, 0.0); |
| 167 | + } |
| 168 | + height_scans_[sensor_name] = std::move(scan); |
| 169 | + return true; |
| 170 | + } |
| 171 | + |
| 172 | + std::optional<HeightScan*> heightScan( |
| 173 | + const std::string& sensor_name, |
| 174 | + const std::unordered_set<std::string>& /*layer_names*/, |
| 175 | + const Position& /*base_pos_w*/, |
| 176 | + const Quaternion& /*base_quat_w*/) override { |
| 177 | + if (!height_scans_.contains(sensor_name)) { |
| 178 | + return std::nullopt; |
| 179 | + } |
| 180 | + return &height_scans_.at(sensor_name); |
| 181 | + } |
| 182 | + |
| 183 | + private: |
| 184 | + std::unordered_map<std::string, JointState> joint_states_; |
| 185 | + std::unordered_map<std::string, SE2Velocity> se2_velocities_; |
| 186 | + std::unordered_map<std::string, HeightScan> height_scans_; |
| 187 | +}; |
| 188 | + |
| 189 | +} // namespace exploy::control::examples |
0 commit comments