C++ Controller API¶
High-performance C++ controller for real-time policy execution with ONNX Runtime.
Overview¶
The C++ controller provides a real-time inference system for deploying RL policies exported to ONNX format. It features:
ONNX Runtime Integration - Efficient model execution
Extensible Interface Design - Easy integration with different robot platforms
Component-Based Architecture - Modular input/output handling
Automatic Component Matching - Maps ONNX model I/O to robot interfaces
Core Classes¶
Controller¶
-
class OnnxRLController¶
Controller wrapping around an ONNX policy.
This class can be used to control a robot with an ONNX policy.
Public Functions
-
explicit OnnxRLController(RobotStateInterface &state, CommandInterface &command, DataCollectionInterface &data_collection)¶
Constructor of the OnnxRLController.
- Parameters:
state – A RobotStateInterface to communicate with the robot.
command – A CommandInterface to send commands to the controller.
data_collection – A DataCollection interface for data collection.
-
bool create(const std::string &onnx_model_path)¶
Create the ONNX model and context.
This function only parses the configuration needed to interface with the ONNX model. Call init() to fully initialize the controller.
- Parameters:
onnx_model_path – Path to the ONNX model file.
- Returns:
True if parsing succeeds, false otherwise.
-
bool init(bool enable_data_collection)¶
Initialize the controller.
- Parameters:
enable_data_collection – Whether to enable data collection.
- Returns:
True if initialization succeeds, false otherwise.
-
void reset()¶
Reset the controller.
-
bool update(uint64_t time_us)¶
Update the controller.
Read from state, evaluate ONNX model and write to state. Should be run at updateRate()
- Parameters:
time_us – Timestamp in microseconds.
- Returns:
True if update succeeds, false otherwise.
-
inline OnnxContext &context()¶
-
explicit OnnxRLController(RobotStateInterface &state, CommandInterface &command, DataCollectionInterface &data_collection)¶
Context Management¶
-
class OnnxContext¶
Manages the context for ONNX model input/output components.
OnnxContext is responsible for parsing ONNX model metadata, matching tensor names to appropriate input/output components using registered matchers, and creating the component instances needed to interface between the robot and the ONNX model.
Public Functions
-
void registerMatcher(std::unique_ptr<Matcher> matcher)¶
Register a matcher for single tensor patterns.
Matchers are used to identify ONNX tensor names and create corresponding input/output components.
- Parameters:
matcher – Unique pointer to a Matcher instance.
-
void registerGroupMatcher(std::unique_ptr<GroupMatcher> matcher)¶
Register a group matcher for multi-tensor patterns.
Group matchers handle patterns where multiple related tensors need to be processed together (e.g., joint.pos and joint.vel).
- Parameters:
matcher – Unique pointer to a GroupMatcher instance.
-
bool createContext(OnnxRuntime &onnx_model, bool strict = true)¶
Create the context by parsing ONNX model and generating components.
Analyzes the ONNX model’s input/output tensors and metadata, matches them against registered matchers, and creates the appropriate input/output components.
- Parameters:
onnx_model – Reference to an initialized OnnxRuntime instance.
strict – If true, fails if any tensor cannot be matched; if false, continues with partial matches.
- Returns:
true if context creation succeeded, false otherwise.
-
inline const std::vector<std::unique_ptr<Input>> &getInputs() const¶
Get all created input components.
- Returns:
Const reference to vector of input component unique pointers.
-
inline const std::vector<std::unique_ptr<Output>> &getOutputs() const¶
Get all created output components.
- Returns:
Const reference to vector of output component unique pointers.
-
inline int updateRate() const¶
Get the control loop update rate from ONNX model metadata.
- Returns:
Update rate in Hz, or 0 if not specified in model metadata.
-
void registerMatcher(std::unique_ptr<Matcher> matcher)¶
ONNX Runtime¶
-
class OnnxRuntime¶
A class for evaluating ONNX models using ONNX Runtime.
The
OnnxRuntimeclass manages the initialization, configuration, and inference execution of an ONNX model.Public Functions
-
bool initialize(const std::string &model_path, const OnnxRuntimeOptions &options = {})¶
Initializes the ONNX model and configures input/output tensors.
Loads the ONNX model from the specified file path, sets up session options, and prepares memory information. If the model file does not exist, the initialization will fail.
- Parameters:
model_path – The file path to the ONNX model.
options – The runtime options.
- Returns:
True if initialization is successful, false otherwise.
-
bool evaluate()¶
Runs inference on the ONNX model with the buffered input data.
This method runs inference and updates the output buffers.
- Returns:
True evaluation succeeds, false if validation fails.
-
std::optional<std::string> getCustomMetadata(const std::string &key) const¶
Getter for custom metadata from ONNX model.
- Parameters:
key – The key of the custom metadata stored in the ONNX model.
- Returns:
A string with the corresponding custom metadata, if the key exists, nullopt otherwise.
-
template<typename T>
inline std::optional<std::span<T>> inputBuffer(const std::string &name)¶ Retrieves a mutable span to the input tensor buffer of the specified name.
- Template Parameters:
T – The data type of the tensor elements (e.g., float, int32_t, bool).
- Parameters:
name – The name of the input tensor.
- Returns:
An optional span to the tensor buffer if it exists and matches the requested type, nullopt otherwise.
-
template<typename T>
inline std::optional<std::span<T>> outputBuffer(const std::string &name)¶ Retrieves a mutable span to the output tensor buffer of the specified name.
- Template Parameters:
T – The data type of the tensor elements (e.g., float, int32_t, bool).
- Parameters:
name – The name of the output tensor.
- Returns:
An optional span to the tensor buffer if it exists and matches the requested type, nullopt otherwise.
-
void resetBuffers()¶
Resets all input and output buffers to zero.
-
std::unordered_set<std::string> inputNames() const¶
Retrieves the set of input tensor names.
- Returns:
An unordered set containing the names of all input tensors.
-
std::unordered_set<std::string> outputNames() const¶
Retrieves the set of output tensor names.
- Returns:
An unordered set containing the names of all output tensors.
-
inline bool isInitialized() const¶
Checks if the ONNX runtime is properly initialized.
- Returns:
True if the runtime has been initialized with a valid model, false otherwise.
-
bool copyOutputToInput(const std::string &output_name, const std::string &input_name)¶
Copies the output tensor data to the input tensor.
- Parameters:
output_name – The name of the output tensor.
input_name – The name of the input tensor.
- Returns:
True if the copy was successful, false otherwise.
-
bool initialize(const std::string &model_path, const OnnxRuntimeOptions &options = {})¶
Interfaces¶
These abstract interfaces define the contract for robot integration.
State Interface¶
-
class RobotStateInterface¶
Interface which provides methods to communicate with the robot.
This class describes the interface for the communication with the robot state and commands.
Public Functions
-
virtual ~RobotStateInterface() = default¶
-
inline virtual bool initBasePosW()¶
Initialize data source of base position in world frame (x,y,z).
Called once during initialization (usually non real-time).
-
inline virtual bool initBaseQuatW()¶
Initialize data source of base orientation in world frame (w,x,y,z).
Called once during initialization (usually non real-time).
-
inline virtual std::optional<Position> basePosW() const¶
Get base position in world frame.
- Returns:
The base position in world frame (x,y,z).
-
inline virtual std::optional<Quaternion> baseQuatW() const¶
Get base orientation in world frame.
- Returns:
The base orientation in world frame (w,x,y,z).
-
inline virtual bool initBaseLinVelB()¶
Initialize data source of linear base velocity in base frame.
Called once during initialization (usually non real-time).
-
inline virtual bool initBaseAngVelB()¶
Initialize data source of angular base velocity in base frame.
Called once during initialization (usually non real-time).
-
inline virtual std::optional<LinearVelocity> baseLinVelB() const¶
Get linear base velocity rotated in base frame.
- Returns:
The linear base velocity in base frame (vx, vy, vz).
-
inline virtual std::optional<AngularVelocity> baseAngVelB() const¶
Get angular base velocity rotated in base frame.
- Returns:
The angular base velocity in base frame (ωx, ωy, ωz).
-
inline virtual bool initJointPosition(const std::string &joint_name)¶
Initialize data sources for joint position data.
Called once during initialization (non real-time).
- Parameters:
joint_name – The name of the joint.
-
inline virtual bool initJointVelocity(const std::string &joint_name)¶
Initialize data sources for joint velocity data.
Called once during initialization (non real-time).
- Parameters:
joint_name – The name of the joint.
-
inline virtual bool initJointEffort(const std::string &joint_name)¶
Initialize data sources for joint effort data.
Called once during initialization (non real-time).
- Parameters:
joint_name – The name of the joint.
-
inline virtual bool initJointOutput(const std::string &joint_name)¶
Initialize data sinks for joint output data.
Called once during initialization (non real-time).
- Parameters:
joint_name – The name of the joint.
-
inline virtual std::optional<double> jointPosition(const std::string &joint_name) const¶
Get joint position.
- Parameters:
joint_name – The name of the joint.
- Returns:
The position of the joint.
-
inline virtual std::optional<double> jointVelocity(const std::string &joint_name) const¶
Get joint velocity.
- Parameters:
joint_name – The name of the joint.
- Returns:
The velocity of the joint.
-
inline virtual std::optional<double> jointEffort(const std::string &joint_name) const¶
Get joint effort.
- Parameters:
joint_name – The name of the joint.
- Returns:
The effort of the joint.
-
inline virtual bool setJointPosition(const std::string &joint_name, double position)¶
Set joint position.
The following control law is assumed: u = joint_effort + p_gain * (joint_position - joint_position_measured) + d_gain * (joint_velocity - joint_velocity_measured)
- Parameters:
joint_name – The name of the joint.
position – The joint position to be set.
-
inline virtual bool setJointVelocity(const std::string &joint_name, double velocity)¶
Set joint velocity.
The following control law is assumed: u = joint_effort + p_gain * (joint_position - joint_position_measured) + d_gain * (joint_velocity - joint_velocity_measured)
- Parameters:
joint_name – The name of the joint.
velocity – The joint velocity to be set.
-
inline virtual bool setJointEffort(const std::string &joint_name, double effort)¶
Set joint effort.
The following control law is assumed: u = joint_effort + p_gain * (joint_position - joint_position_measured) + d_gain * (joint_velocity - joint_velocity_measured)
- Parameters:
joint_name – The name of the joint.
effort – The joint effort to be set.
-
inline virtual bool setJointPGain(const std::string &joint_name, double p_gain)¶
Set joint p-gain.
The following control law is assumed: u = joint_effort + p_gain * (joint_position - joint_position_measured) + d_gain * (joint_velocity - joint_velocity_measured)
- Parameters:
joint_name – The name of the joint.
p_gain – The p-gain to be set.
-
inline virtual bool setJointDGain(const std::string &joint_name, double d_gain)¶
Set joint d-gain.
The following control law is assumed: u = joint_effort + p_gain * (joint_position - joint_position_measured) + d_gain * (joint_velocity - joint_velocity_measured)
- Parameters:
joint_name – The name of the joint.
d_gain – The d-gain to be set.
-
inline virtual bool initSe2Velocity(const std::string &frame_name)¶
Initialize data sources of se(2) frame velocity.
Called once during initialization (non real-time).
- Parameters:
frame_name – The name of the considered frame.
-
inline virtual bool setSe2Velocity(const std::string &frame_name, const SE2Velocity&)¶
Set se(2) velocity reference of a specific frame.
- Parameters:
velocity – The se(2) velocity to be set.
frame_name – The name of the considered frame.
-
inline virtual bool initImuAngularVelocityImu(const std::string &imu_name)¶
Initialize data source of IMU angular velocity.
Called once during initialization (usually non real-time).
- Parameters:
imu_name – The name of the IMU.
-
inline virtual bool initImuOrientationW(const std::string &imu_name)¶
Initialize data source of IMU orientation.
Called once during initialization (usually non real-time).
- Parameters:
imu_name – The name of the IMU.
-
inline virtual std::optional<AngularVelocity> imuAngularVelocityImu(const std::string &imu_name) const¶
Get IMU angular velocity in IMU frame.
- Parameters:
imu_name – The name of the IMU.
- Returns:
The angular velocity of the IMU in world frame (x,y,z) .
-
inline virtual std::optional<Quaternion> imuOrientationW(const std::string &imu_name) const¶
Get IMU orientation in world frame (w, x, y, z).
- Parameters:
imu_name – The name of the IMU.
- Returns:
The orientation of the IMU in world frame represented as a unit quaternion (w,x,y,z).
-
inline virtual bool initBodyPositionW(const std::string &body_name)¶
Initialize data source of body position in world frame.
Called once during initialization (usually non real-time).
- Parameters:
body_name – The name of the body.
-
inline virtual bool initBodyOrientationW(const std::string &body_name)¶
Initialize data source of body orientation in base frame (w, x, y, z).
Called once during initialization (usually non real-time).
- Parameters:
body_name – The name of the body.
-
inline virtual bool initBodyLinearVelocityB(const std::string &body_name)¶
Initialize data source of body linear velocity in body frame.
Called once during initialization (usually non real-time).
- Parameters:
body_name – The name of the body.
-
inline virtual bool initBodyAngularVelocityB(const std::string &body_name)¶
Initialize data source of body angular velocity in body frame.
Called once during initialization (usually non real-time).
- Parameters:
body_name – The name of the body.
-
inline virtual std::optional<Position> bodyPositionW(const std::string &body_name) const¶
Get body position in world frame.
- Parameters:
body_name – The name of the body.
- Returns:
The position of the body in world frame.
-
inline virtual std::optional<Quaternion> bodyOrientationW(const std::string &body_name) const¶
Get body orientation in world frame (w, x, y, z).
- Parameters:
body_name – The name of the body.
- Returns:
The orientation of the body in world frame.
-
inline virtual std::optional<LinearVelocity> bodyLinearVelocityB(const std::string &body_name) const¶
Get body linear velocity in body frame.
- Parameters:
body_name – The name of the body.
- Returns:
The linear velocity of the body in body frame.
-
inline virtual std::optional<AngularVelocity> bodyAngularVelocityB(const std::string &body_name) const¶
Get body angular velocity in body frame.
- Parameters:
body_name – The name of the body.
- Returns:
The angular velocity of the body in body frame.
-
inline virtual bool initHeightScan(const std::string&, const HeightScanConfig&)¶
Initialize data source of the heightscan.
Called once during initialization (usually non real-time).
- Parameters:
config – The configuration of the heightscan.
-
inline virtual std::optional<HeightScan*> heightScan(const std::string&, const std::unordered_set<std::string>&, const Position&, const Quaternion&)¶
Get subsampled heightscan in base frame.
This function returns the flattened (column-major), subsampled heightscan with the pattern specified in the init function. The pattern is centered around the base.
- Parameters:
base_pos_w – The base position in world frame.
base_quat_w – The base orientation in world frame.
- Returns:
A vector of heightscans.
-
inline virtual bool initSphericalImage(const std::string&, const SphericalImageConfig&)¶
Initialize the spherical image data source.
Called once during initialization (usually non real-time).
- Parameters:
sensor_name – The name of the spherical image sensor.
config – The configuration of the spherical image.
-
inline virtual std::optional<MultiChannelImage*> sphericalImage(const std::string&, const std::unordered_set<std::string>&)¶
Get the spherical image with multiple channels.
This function returns the multi-channel spherical image data, with each channel flattened in row-major storage order.
- Parameters:
sensor_name – The name of the spherical image sensor.
channel_names – The set of channel names to retrieve.
- Returns:
Pointer to the spherical image data.
-
inline virtual bool initPinholeImage(const std::string&, const PinholeImageConfig&)¶
Initialize the pinhole image data source.
Called once during initialization (usually non real-time).
- Parameters:
sensor_name – The name of the pinhole image sensor.
config – The configuration of the pinhole image.
-
inline virtual std::optional<MultiChannelImage*> pinholeImage(const std::string&, const std::unordered_set<std::string>&)¶
Get the pinhole image with multiple channels.
This function returns the multi-channel pinhole image data, with each channel flattened in row-major storage order.
- Parameters:
sensor_name – The name of the pinhole image sensor.
channel_names – The set of channel names to retrieve.
- Returns:
Pointer to the pinhole image data.
-
virtual ~RobotStateInterface() = default¶
Command Interface¶
-
class CommandInterface¶
Interface which provides methods to send commands to the controllers.
Public Functions
-
virtual ~CommandInterface() = default¶
-
inline virtual bool initSe2Velocity(const std::string &command_name, const SE2VelocityConfig&)¶
Initialize data source of commanded se2 velocity.
Called once during initialization (usually non real-time).
- Parameters:
command_name – The name of the command.
config – The configuration for the commanded se2 velocity.
- Returns:
True if initialization succeeded, false otherwise.
-
inline virtual std::optional<SE2Velocity> se2Velocity(const std::string &command_name)¶
Get commanded se2 velocity.
- Parameters:
command_name – The name of the command.
- Returns:
The commanded se2 velocity (lin x, lin y, ang z).
-
inline virtual bool initSe3Pose(const std::string &command_name, const SE3PoseConfig&)¶
Initialize data source of commanded SE3 pose.
Called once during initialization (usually non real-time).
- Parameters:
command_name – The name of the command.
config – The configuration for the commanded SE3 pose.
- Returns:
True if initialization succeeded, false otherwise.
-
inline virtual std::optional<SE3Pose> se3Pose(const std::string &command_name) const¶
Get commanded SE3 some pose.
- Parameters:
command_name – The name of the command.
- Returns:
The commanded SE3 pose.
-
inline virtual bool initBooleanSelector(const std::string &command_name, const BooleanSelectorConfig&)¶
Initialize data source of a boolean.
Called once during initialization (usually non real-time).
- Parameters:
command_name – The name of the command.
config – The configuration for the commanded boolean selector.
- Returns:
True if initialization succeeded, false otherwise.
-
inline virtual std::optional<bool> booleanSelector(const std::string &command_name) const¶
Get commanded boolean selector.
- Parameters:
command_name – The name of the command.
- Returns:
The commanded bool.
-
inline virtual bool initFloatValue(const std::string &command_name, const FloatScalarConfig&)¶
Initialize data source of a float.
Called once during initialization (usually non real-time).
- Parameters:
command_name – The name of the command.
config – The configuration for the commanded float scalar.
- Returns:
True if initialization succeeded, false otherwise.
-
inline virtual std::optional<float> floatValue(const std::string &command_name) const¶
Get commanded float value.
- Parameters:
command_name – The name of the command.
- Returns:
The commanded float.
-
virtual ~CommandInterface() = default¶
Data Collection Interface¶
-
class DataCollectionInterface¶
Interface which provides methods to collect data.
Public Functions
-
virtual ~DataCollectionInterface() = default¶
-
inline virtual bool registerDataSource(const std::string&, std::span<const double>)¶
Register a span data source for data collection.
- Parameters:
prefix – A prefix to identify the data.
data – The data to be logged.
-
inline virtual bool registerDataSource(const std::string&, std::span<const float>)¶
Register a span data source for data collection.
- Parameters:
prefix – A prefix to identify the data.
data – The data to be logged.
-
inline virtual bool registerDataSource(const std::string&, const double&)¶
Register a scalar data source for data collection.
- Parameters:
prefix – A prefix to identify the data.
data – The data to be logged.
-
virtual bool collectData(uint64_t time_us) = 0¶
Collect registered data.
- Parameters:
time_us – Timestamp in microseconds.
- Returns:
true if data collection succeeded, false otherwise.
-
virtual ~DataCollectionInterface() = default¶
Components¶
Input and output components for data flow management.
-
struct Input¶
Abstract base class for input components that read data into ONNX model inputs.
Input components read robot state, sensor data, or commands and populate ONNX runtime input tensors. Subclasses implement specific data sources (joints, IMU, cameras, etc.).
Subclassed by exploy::control::BaseAngularVelocityInput, exploy::control::BaseLinearVelocityInput, exploy::control::BaseOrientationInput, exploy::control::BasePositionInput, exploy::control::BodyOrientationInput, exploy::control::BodyPositionInput, exploy::control::CommandBooleanInput, exploy::control::CommandFloatInput, exploy::control::CommandSE2VelocityInput, exploy::control::CommandSE3PoseInput, exploy::control::HeightScanInput, exploy::control::IMUAngularVelocityInput, exploy::control::IMUOrientationInput, exploy::control::JointPositionInput, exploy::control::JointVelocityInput, exploy::control::PinholeImageInput, exploy::control::SphericalImageInput, exploy::control::StepCountInput
Public Functions
-
virtual ~Input() = default¶
-
inline virtual bool init(RobotStateInterface&, CommandInterface&)¶
Initialize the input component (non-real-time).
Called once during controller setup to register data sources and configure parameters.
- Parameters:
state – Robot state interface for reading sensor and state data.
command – Command interface for reading external commands.
- Returns:
true if initialization succeeded, false otherwise.
-
virtual bool read(OnnxRuntime &runtime, RobotStateInterface &state, CommandInterface &command) = 0¶
Read data from robot/command interface into ONNX input buffer (real-time).
Called every control loop to update the ONNX model inputs with current data.
- Parameters:
runtime – ONNX runtime containing input/output buffers.
state – Robot state interface for reading sensor and state data.
command – Command interface for reading external commands.
- Returns:
true if read succeeded, false if data unavailable.
-
virtual ~Input() = default¶
-
struct Output¶
Abstract base class for output components that write ONNX model outputs.
Output components read ONNX runtime output tensors and write commands to robot control interfaces. Subclasses implement specific output targets (joint targets, velocity commands, memory buffers, etc.).
Subclassed by exploy::control::JointTargetOutput, exploy::control::MemoryOutput, exploy::control::SE2VelocityOutput
Public Functions
-
virtual ~Output() = default¶
-
inline virtual bool init(RobotStateInterface&, CommandInterface&)¶
Initialize the output component (non-real-time).
Called once during controller setup to configure output targets.
- Parameters:
state – Robot state interface for accessing robot configuration.
command – Command interface for configuring output targets.
- Returns:
true if initialization succeeded, false otherwise.
-
virtual bool write(OnnxRuntime &runtime, RobotStateInterface &state, CommandInterface &command) = 0¶
Write ONNX output buffer to robot/command interface (real-time).
Called every control loop after ONNX inference to apply model outputs.
- Parameters:
runtime – ONNX runtime containing input/output buffers.
state – Robot state interface for writing state commands.
command – Command interface for writing control commands.
- Returns:
true if write succeeded, false otherwise.
-
virtual ~Output() = default¶
Data Types¶
Core data structures and type aliases used throughout the controller.
-
using exploy::control::Position = Eigen::Vector3d¶
3D position vector.
Represents a position in 3D Cartesian space using Eigen’s Vector3d.
-
using exploy::control::Quaternion = Eigen::Quaterniond¶
Unit quaternion for 3D orientation.
Represents a rotation in 3D space using Eigen’s quaternion representation. The quaternion should be normalized (unit quaternion).
-
using exploy::control::LinearVelocity = Eigen::Vector3d¶
3D linear velocity vector.
Represents linear velocity in 3D Cartesian space.
-
using exploy::control::AngularVelocity = Eigen::Vector3d¶
3D angular velocity vector.
Represents angular velocity in 3D space (rotation rates around x, y, z axes).
-
using exploy::control::SE2Velocity = Eigen::Vector3d¶
SE(2) velocity vector [v_x, v_y, omega_z].
Represents planar velocity with linear components in x and y directions and angular velocity around the z-axis.
-
struct SE3Pose¶
SE(3) pose representation.
Represents a 6-DOF pose in 3D space combining position and orientation.
Public Members
-
Quaternion orientation¶
Orientation as unit quaternion.
-
Quaternion orientation¶
-
struct HeightScan¶
A flattened height scan with multiple layers.
The height scan was flattened from a grid pattern according to IsaacLab conventions. Each layer represents different terrain properties (e.g., height, color, normals). All layers come from the same grid pattern and must have the same length.
Note
As soon as the ONNX wrapper supports multi-dimensional inputs, this should be changed to a 2D array representation for better performance and clarity.
Public Members
-
std::unordered_map<std::string, std::span<const float>> float_layers¶
Map of layer names to their data spans.
Each entry maps a layer name (e.g., “height”, “color”) to a span of float values representing the flattened grid data for that layer.
-
std::unordered_map<std::string, std::span<const float>> float_layers¶
Configuration¶
-
struct OnnxRuntimeOptions¶
Configuration options for initializing the ONNX Runtime.
This struct encapsulates all configuration parameters needed to initialize an ONNX model.
Public Members
-
ExecutionProvider provider = ExecutionProvider::CPU¶
-
std::optional<std::string> profiling_path = std::nullopt¶
-
ExecutionProvider provider = ExecutionProvider::CPU¶
-
struct SE2VelocityConfig¶
Configuration for SE2 velocity commands.
Public Members
-
std::optional<SE2VelocityRanges> ranges = {}¶
-
std::optional<SE2VelocityRanges> ranges = {}¶
-
struct SE2VelocityRanges¶
SE(2) velocity constraints.
Defines the allowable ranges for planar velocity commands including linear velocities in x and y directions and angular velocity around z-axis.
Metadata¶
Metadata structures for component configuration.
-
struct JointMetadata¶
Metadata for joint configurations.
Specifies the list of joint names for joint-related inputs.
Public Members
-
std::vector<std::string> names = {}¶
Joint names.
-
std::vector<std::string> names = {}¶
-
struct JointOutputMetadata¶
Metadata for joint output commands.
Specifies joint names and PD controller gains (stiffness and damping) for position-controlled joints.
-
struct SE2VelocityCommandMetadata¶
Metadata for SE(2) velocity commands.
Specifies optional constraints on planar velocity commands including linear and angular velocity ranges.
Public Members
-
std::optional<SE2VelocityRanges> ranges = {}¶
Optional velocity range constraints.
-
std::optional<SE2VelocityRanges> ranges = {}¶
-
struct HeightScanMetadata¶
Metadata for height scan sensors.
Specifies the grid pattern configuration for terrain height scanning including resolution, size, and offset relative to the base frame.
Public Members
-
std::string pattern_type = {}¶
Grid pattern type (e.g., “grid”, “radial”).
-
double resolution = {}¶
Grid resolution (spacing between points).
-
double size_x = {}¶
Grid size in x direction.
-
double size_y = {}¶
Grid size in y direction.
-
double offset_x = {}¶
Grid offset in x direction from base.
-
double offset_y = {}¶
Grid offset in y direction from base.
-
std::string pattern_type = {}¶
-
struct SphericalImageMetadata¶
Metadata for spherical image sensors (e.g., LiDAR).
Specifies the configuration for spherical images including resolution, field of view, and sentinel value for unobserved points.
Public Members
-
std::string pattern_type = {}¶
Spherical image pattern type.
-
int v_res = {}¶
Vertical resolution (number of vertical scan lines).
-
int h_res = {}¶
Horizontal resolution (points per scan line).
-
double v_fov_min_deg = {}¶
Minimum vertical field of view in degrees.
-
double v_fov_max_deg = {}¶
Maximum vertical field of view in degrees.
-
double unobserved_value = {}¶
Sentinel value for unobserved/invalid points.
-
std::string pattern_type = {}¶
-
struct PinholeImageMetadata¶
Metadata for pinhole camera image sensors.
Specifies camera configuration including image dimensions and intrinsic parameters.
Public Members
-
std::string pattern_type = {}¶
Pinhole image pattern type.
-
int width = {}¶
Image width in pixels.
-
int height = {}¶
Image height in pixels.
-
double fx = {}¶
Focal length in x direction (pixels).
-
double fy = {}¶
Focal length in y direction (pixels).
-
double cx = {}¶
Principal point x-coordinate (pixels).
-
double cy = {}¶
Principal point y-coordinate (pixels).
-
std::string pattern_type = {}¶
Matchers¶
Component matchers for automatic ONNX I/O mapping.
-
class Matcher¶
Abstract base class for single-tensor pattern matching.
Matcher implementations recognize specific ONNX tensor name patterns and create corresponding input/output components. Each matcher handles one type of component.
Subclassed by exploy::control::BaseAngularVelocityMatcher, exploy::control::BaseLinearVelocityMatcher, exploy::control::BaseOrientationMatcher, exploy::control::BasePositionMatcher, exploy::control::BodyOrientationMatcher, exploy::control::BodyPositionMatcher, exploy::control::CommandBooleanMatcher, exploy::control::CommandFloatMatcher, exploy::control::CommandSE2VelocityMatcher, exploy::control::CommandSE3PoseMatcher, exploy::control::IMUAngularVelocityMatcher, exploy::control::IMUOrientationMatcher, exploy::control::SE2VelocityMatcher, exploy::control::StepCountMatcher
Public Functions
-
virtual ~Matcher() = default¶
-
virtual bool matches(const Match &maybe_match) = 0¶
Check if a tensor matches this matcher’s pattern.
- Parameters:
maybe_match – Potential match containing tensor name and metadata.
- Returns:
true if the tensor matches and was stored, false otherwise.
-
virtual ~Matcher() = default¶
-
struct Match¶
Represents a single matched ONNX tensor.
Contains the tensor name and optional metadata associated with the tensor from the ONNX model.