mock_pendulum_hardware.hpp

// Copyright 2026 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef MOCK_PENDULUM_HARDWARE__MOCK_PENDULUM_HARDWARE_HPP_
#define MOCK_PENDULUM_HARDWARE__MOCK_PENDULUM_HARDWARE_HPP_
 
#include <memory>
#include <random>
#include <string>
#include <vector>
 
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
 
namespace mock_pendulum_hardware
{
 
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
class MockPendulumHardware : public hardware_interface::SystemInterface
{
public:
  RCLCPP_SHARED_PTR_DEFINITIONS(MockPendulumHardware)
 
  CallbackReturn on_init(
    const hardware_interface::HardwareComponentInterfaceParams & params) override;
 
  std::vector<hardware_interface::StateInterface::ConstSharedPtr> on_export_state_interfaces()
    override;
  std::vector<hardware_interface::CommandInterface::SharedPtr> on_export_command_interfaces()
    override;
 
  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
  hardware_interface::return_type read(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;
  hardware_interface::return_type write(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
private:
  // ---- parameter helpers -------------------------------------------------
  double param(const std::string & name, double fallback) const;
  bool param_bool(const std::string & name, bool fallback) const;
 
  // ---- plant -------------------------------------------------------------
  double pendulum_accel(double q2, double q2_dot, double q1_dot, double q1_ddot) const;
 
  void integrate(double h);
 
  void publish_states(double dt_sim);
 
  static double wrap_pi(double a);
 
  // ---- physical parameters (post randomisation) --------------------------
  double J1_{6.10e-5};         
  double J2_pivot_{4.447e-5};  
  double J2_com_{8.06e-6};     
  double m2_{0.014};           
  double l2_{0.051};           
  double L1_{0.062};           
  double b1_{1.0e-4};          
  double b2_{8.0e-6};          
  double c2_{2.0e-6};          
  double g_{9.80665};
 
  // nominal (pre-randomisation) copies, so re-activation re-rolls from nominal
  double J1_nom_{0.0}, J2_com_nom_{0.0}, m2_nom_{0.0}, l2_nom_{0.0};
  double b1_nom_{0.0}, b2_nom_{0.0}, c2_nom_{0.0};
 
  // ---- actuator limits ---------------------------------------------------
  double motor_limit_{2.35619449};  
  double motor_max_vel_{5.0};       
  double motor_max_accel_{200.0};   
  double motor_max_torque_{0.4};    
  bool model_step_loss_{true};
  double stop_restitution_{0.0};  
 
  // ---- sensor model ------------------------------------------------------
  double motor_steps_per_rev_{6400.0};
  double encoder_counts_per_rev_{4096.0};
  double encoder_noise_{0.0002};  
  double encoder_offset_{0.0};    
  bool pendulum_vel_from_encoder_{true};
  double vel_filter_alpha_{0.95};
  double motor_vel_noise_{0.001};
  double report_offset_{0.0};  
 
  // ---- process noise -----------------------------------------------------
  double pendulum_accel_noise_{
    0.005};                          
  double motor_accel_noise_{0.005};  
  double param_randomization_{0.005};  
  double initial_q2_{0.1};             
  double initial_q2_jitter_{0.002};    
 
  // ---- integration / timing ---------------------------------------------
  double sim_speed_{1.0};  
  double max_substep_{5.0e-4};
  double max_cycle_dt_{0.1};
  int max_substeps_{4000};
 
  // ---- true state --------------------------------------------------------
  double q1_{0.0}, q1_dot_{0.0};
  double q2_{0.0}, q2_dot_{0.0};
  double q2_ddot_prev_{0.0};
  double sim_time_{0.0};
  bool at_limit_{false};
  bool step_loss_{false};
 
  // ---- measured state ----------------------------------------------------
  double q2_meas_prev_{0.0};
  double q2_dot_filt_{0.0};
  bool have_prev_meas_{false};
 
  std::vector<double> hw_states_;
  std::vector<double> hw_commands_;
 
  std::vector<hardware_interface::StateInterface::SharedPtr> state_interfaces_;
  std::vector<hardware_interface::CommandInterface::SharedPtr> command_interfaces_;
 
  rclcpp::Clock clock_{RCL_STEADY_TIME};
 
  std::mt19937 rng_;
  std::normal_distribution<double> normal_{0.0, 1.0};
};
 
}  // namespace mock_pendulum_hardware
 
#endif  // MOCK_PENDULUM_HARDWARE__MOCK_PENDULUM_HARDWARE_HPP_