46 controller_interface::CallbackReturn
on_init()
override;
52 controller_interface::CallbackReturn on_configure(
53 const rclcpp_lifecycle::State & previous_state)
override;
55 controller_interface::CallbackReturn on_activate(
56 const rclcpp_lifecycle::State & previous_state)
override;
58 controller_interface::CallbackReturn on_deactivate(
59 const rclcpp_lifecycle::State & previous_state)
override;
61 controller_interface::return_type
update(
62 const rclcpp::Time & time,
const rclcpp::Duration & period)
override;
69 const ros2_control_demo_example_18::msg::VelocityCommandWithHead & msg);
89 const std::vector<int64_t> & model_shape,
size_t input_size);
92 bool load_model(
const std::string & model_path);
93 bool run_model_inference(
const std::vector<float> & inputs, std::vector<double> & outputs);
94 void initialize_motor_targets_from_defaults();
95 size_t write_commands_to_hardware(
const std::vector<double> & joint_commands);
96 void apply_blend_in(std::vector<double> & joint_commands,
double blend_factor);
97 std::vector<bool> apply_rate_limiting(
98 std::vector<double> & joint_commands,
double control_period);
99 std::vector<bool> apply_position_limiting(std::vector<double> & joint_commands);
101 void validate_model_structure(
size_t num_inputs,
size_t num_outputs);
103 std::shared_ptr<ParamListener> param_listener_;
106 std::vector<std::string> joint_names_;
107 std::string model_path_;
108 int model_input_size_;
109 int model_output_size_;
110 std::string interfaces_broadcaster_topic_;
111 std::string interfaces_broadcaster_names_topic_;
112 std::string velocity_command_topic_;
113 std::string left_contact_sensor_name_{
"left_foot_contact"};
114 std::string right_contact_sensor_name_{
"right_foot_contact"};
116 std::unique_ptr<Ort::Session> onnx_session_;
117 std::unique_ptr<Ort::Env> onnx_env_;
118 std::unique_ptr<Ort::MemoryInfo> onnx_memory_info_;
119 std::vector<std::string> input_names_;
120 std::vector<std::string> output_names_;
121 std::vector<const char *> input_name_ptrs_;
122 std::vector<const char *> output_name_ptrs_;
123 std::vector<int64_t> input_shape_;
124 std::vector<int64_t> output_shape_;
127 rclcpp::Subscription<control_msgs::msg::Float64Values>::SharedPtr interface_data_subscriber_;
128 rclcpp::Subscription<control_msgs::msg::Keys>::SharedPtr interfaces_names_subscriber_;
129 rclcpp::Subscription<ros2_control_demo_example_18::msg::VelocityCommandWithHead>::SharedPtr
130 velocity_command_subscriber_;
135 rt_velocity_command_;
138 std::atomic<bool> interface_data_received_{
false};
139 std::atomic<bool> velocity_command_received_{
false};
140 ros2_control_demo_example_18::msg::VelocityCommandWithHead velocity_cmd_;
144 std::unique_ptr<ObservationFormatter> observation_formatter_;
145 std::unique_ptr<ActionProcessor> action_processor_;
146 std::vector<double> previous_action_;
149 std::vector<std::string> command_interface_names_;
150 std::vector<double> default_joint_positions_;
151 bool default_joint_positions_initialized_;
152 double max_motor_velocity_;
153 std::vector<double> joint_position_limits_min_;
154 std::vector<double> joint_position_limits_max_;
155 bool joint_position_limits_enabled_{
false};
156 std::vector<double> motor_targets_;
157 std::vector<double> prev_motor_targets_;
158 bool prev_motor_targets_initialized_;
161 double phase_frequency_factor_offset_;
162 double num_steps_in_gait_period_;
163 double training_control_period_;
164 double gyro_deadband_;
165 int stabilization_delay_;
167 size_t stabilization_steps_;
168 size_t onnx_active_steps_;
169 size_t update_count_;