diff --git a/Cargo.toml b/Cargo.toml
index 478cbda..073c2ec 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -1,4 +1,5 @@
[workspace]
+resolver = "2"
members = [
"orbita2d_c_api",
diff --git a/README.md b/README.md
index ab667a6..324d624 100644
--- a/README.md
+++ b/README.md
@@ -32,7 +32,22 @@ See [orbita2d python bindings](orbita2d_c_api/python/) for more details.
#### Setup
* To integrate into a ROS2 workspace, simply clone this repository into the src/ directory of your workspace.
+* Make sure to have the [ros2_pollen_toolbox](https://github.com/pollen-robotics/ros2_pollen_toolbox) in the same workspace
+* Make sure to have [ros2_rust]() setup.
+
+ If its your first time using rust with ros
+ Make sure to install:
+
+ ```
+ pip install git+https://github.com/colcon/colcon-cargo.git
+ pip install git+https://github.com/colcon/colcon-ros-cargo.git
+ ```
+
+
* Make sure to use "patched version" of cargo-ament-built (see [this PR](https://github.com/ros2-rust/cargo-ament-build/pull/3) for more details)
+```
+cargo install --debug --git https://github.com/jerry73204/cargo-ament-build.git --branch conditionally-copy-cargo-lock-file
+```
* Update colcon-cargo with: `python3 -m pip install --upgrade --force-reinstall git+https://github.com/pollen-robotics/colcon-cargo.git`
#### Build the hwi
diff --git a/orbita2d_controller/Cargo.toml b/orbita2d_controller/Cargo.toml
index 8e3d0fb..c475967 100644
--- a/orbita2d_controller/Cargo.toml
+++ b/orbita2d_controller/Cargo.toml
@@ -19,3 +19,4 @@ serde_yaml = "0.9.22"
serde = { version = "1.0.171", features = ["derive"] }
log = { version = "0.4.20", features = ["max_level_debug", "release_max_level_warn"] }
env_logger = "0.9.0"
+timeit = "0.1.2"
diff --git a/orbita2d_controller/src/bin/benchmark.rs b/orbita2d_controller/src/bin/benchmark.rs
new file mode 100644
index 0000000..d47db55
--- /dev/null
+++ b/orbita2d_controller/src/bin/benchmark.rs
@@ -0,0 +1,116 @@
+use std::time::Duration;
+
+use clap::Parser;
+
+#[macro_use]
+extern crate timeit;
+
+/// Benchmark the communication with an Orbita2d
+#[derive(Parser, Debug)]
+#[command(author, version, about, long_about = None)]
+struct Args {
+ /// Name of the person to greet
+ #[arg(short, long)]
+ configfile: String,
+
+ /// Number of iterations
+ #[arg(short, long, default_value = "1000")]
+ iterations: usize,
+}
+
+fn controller_hwi_read_iteration(
+ orbita2d: &mut orbita2d_controller::Orbita2dController,
+) -> Result<(), Box> {
+ let _ = orbita2d.get_current_orientation()?;
+
+ let _ = orbita2d.is_torque_on()?;
+
+ let _ = orbita2d.get_raw_motors_torque_limit()?;
+ let _ = orbita2d.get_raw_motors_velocity_limit()?;
+ let _ = orbita2d.get_raw_motors_pid_gains()?;
+
+ Ok(())
+}
+
+fn controller_hwi_write_iteration(
+ orbita2d: &mut orbita2d_controller::Orbita2dController,
+) -> Result<(), Box> {
+ orbita2d.set_target_orientation([0.0, 0.0])?;
+ orbita2d.enable_torque(false)?;
+
+ Ok(())
+}
+
+fn controller_hwi_iteration(
+ orbita2d: &mut orbita2d_controller::Orbita2dController,
+) -> Result<(), Box> {
+ controller_hwi_read_iteration(orbita2d)?;
+ controller_hwi_write_iteration(orbita2d)?;
+
+ Ok(())
+}
+
+fn main() -> Result<(), Box> {
+ let args = Args::parse();
+
+ println!("Benchmarking Orbita2d:");
+ println!("---------------------:");
+ println!("Results:");
+
+ println!("\tConnecting to Orbita2d: {:?}", args.configfile);
+ let mut orbita2d = orbita2d_controller::Orbita2dController::with_config(&args.configfile)?;
+
+ let dur = Duration::from_secs_f64(timeit_loops!(args.iterations, {
+ let _ = orbita2d.get_current_orientation()?;
+ }));
+ println!(
+ "\tReading current position: {} ms",
+ dur.as_millis() as f64 / args.iterations as f64
+ );
+
+ let dur = Duration::from_secs_f64(timeit_loops!(args.iterations, {
+ controller_hwi_read_iteration(&mut orbita2d)?;
+ }));
+ println!(
+ "\tRead hwi iteration loop: {} ms",
+ dur.as_millis() as f64 / args.iterations as f64
+ );
+
+ let dur = Duration::from_secs_f64(timeit_loops!(args.iterations, {
+ controller_hwi_write_iteration(&mut orbita2d)?;
+ }));
+ println!(
+ "\tWrite hwi iteration loop: {} ms",
+ dur.as_millis() as f64 / args.iterations as f64
+ );
+
+ let dur = Duration::from_secs_f64(timeit_loops!(args.iterations, {
+ controller_hwi_iteration(&mut orbita2d)?;
+ }));
+ println!(
+ "\tRead/Write hwi iteration loop: {} ms",
+ dur.as_millis() as f64 / args.iterations as f64
+ );
+
+ let kin = orbita2d_kinematics::Orbita2dKinematicsModel::new(-47.519, -47.519);
+
+ let thetas = [0.20, -1.0];
+ let dur = Duration::from_secs_f64(timeit_loops!(args.iterations, {
+ kin.compute_forward_kinematics(thetas);
+ }));
+ println!(
+ "\tForward kinematics: {} µs",
+ dur.as_micros() as f64 / args.iterations as f64
+ );
+
+ let rot = kin.compute_forward_kinematics(thetas);
+ let dur = Duration::from_secs_f64(timeit_loops!(args.iterations, {
+ let _ = kin.compute_inverse_kinematics(rot);
+ }));
+ println!(
+ "\tInverse kinematics: {} µs",
+ dur.as_micros() as f64 / args.iterations as f64
+ );
+
+ Ok(())
+}
diff --git a/orbita2d_description/ros2_control/orbita2d.ros2_control.xacro b/orbita2d_description/ros2_control/orbita2d.ros2_control.xacro
index 3785071..d1c32e0 100644
--- a/orbita2d_description/ros2_control/orbita2d.ros2_control.xacro
+++ b/orbita2d_description/ros2_control/orbita2d.ros2_control.xacro
@@ -1,7 +1,10 @@
-
+
+
+
+
@@ -25,8 +28,14 @@
-
-
+
+
+
+
+
+
+
+
diff --git a/orbita2d_description/ros2_control/orbita2d_actuator_control.ros2_control.xacro b/orbita2d_description/ros2_control/orbita2d_actuator_control.ros2_control.xacro
new file mode 100644
index 0000000..92bd719
--- /dev/null
+++ b/orbita2d_description/ros2_control/orbita2d_actuator_control.ros2_control.xacro
@@ -0,0 +1,15 @@
+
+
+
+
+
+
+ 0.0
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/orbita2d_description/ros2_control/orbita2d_axis_control.ros2_control.xacro b/orbita2d_description/ros2_control/orbita2d_axis_control.ros2_control.xacro
new file mode 100644
index 0000000..f0c218a
--- /dev/null
+++ b/orbita2d_description/ros2_control/orbita2d_axis_control.ros2_control.xacro
@@ -0,0 +1,18 @@
+
+
+
+
+
+ 0.0
+
+
+ 0.0
+
+
+ 0.0
+
+
+
+
+
+
\ No newline at end of file
diff --git a/orbita2d_description/ros2_control/orbita2d_raw_motor_control.ros2_control.xacro b/orbita2d_description/ros2_control/orbita2d_raw_motor_control.ros2_control.xacro
new file mode 100644
index 0000000..762458a
--- /dev/null
+++ b/orbita2d_description/ros2_control/orbita2d_raw_motor_control.ros2_control.xacro
@@ -0,0 +1,24 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/orbita2d_system_hwi/include/orbita2d_system_hwi/orbita2d_system_hwi.hpp b/orbita2d_system_hwi/include/orbita2d_system_hwi/orbita2d_system_hwi.hpp
index 364bd39..24554d6 100644
--- a/orbita2d_system_hwi/include/orbita2d_system_hwi/orbita2d_system_hwi.hpp
+++ b/orbita2d_system_hwi/include/orbita2d_system_hwi/orbita2d_system_hwi.hpp
@@ -30,21 +30,22 @@ class Orbita2dSystem : public hardware_interface::SystemInterface
hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override;
private:
+ double hw_states_torque_;
+ double hw_commands_torque_;
+
double hw_states_position_[2];
double hw_states_velocity_[2];
double hw_states_effort_[2];
+ double hw_commands_position_[2];
+
double hw_states_temperature_[2];
double hw_states_torque_limit_[2];
double hw_states_speed_limit_[2];
- double hw_states_torque_[2];
double hw_states_p_gain_[2];
double hw_states_i_gain_[2];
double hw_states_d_gain_[2];
-
- double hw_commands_position_[2];
double hw_commands_speed_limit_[2];
double hw_commands_torque_limit_[2];
- double hw_commands_torque_[2];
double hw_commands_p_gain_[2];
double hw_commands_i_gain_[2];
double hw_commands_d_gain_[2];
diff --git a/orbita2d_system_hwi/src/orbita2d_system_hwi.cpp b/orbita2d_system_hwi/src/orbita2d_system_hwi.cpp
index bfe4961..7005531 100644
--- a/orbita2d_system_hwi/src/orbita2d_system_hwi.cpp
+++ b/orbita2d_system_hwi/src/orbita2d_system_hwi.cpp
@@ -46,6 +46,15 @@ Orbita2dSystem::on_init(const hardware_interface::HardwareInfo & info)
return CallbackReturn::ERROR;
}
+ long unsigned int nb_gpios_expected = 3; // actuator + raw_motor_1 + raw_motor_2
+ if (info.gpios.size() != nb_gpios_expected)
+ {
+ RCLCPP_ERROR(rclcpp::get_logger("Orbita2dSystem"),
+ "Incorrect number of gpios, expected %ld, got \"%s\"", nb_gpios_expected,
+ std::to_string(info.gpios.size()).c_str()
+ );
+ return CallbackReturn::ERROR;
+ }
const char *config_file;
bool from_config=false;
@@ -102,6 +111,9 @@ Orbita2dSystem::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
{
// Set some default values
auto ret= CallbackReturn::SUCCESS;
+
+ hw_states_torque_ = std::numeric_limits::quiet_NaN();
+
for (int i=0; i < 2; i++) {
hw_states_position_[i] = std::numeric_limits::quiet_NaN();
hw_states_velocity_[i] = std::numeric_limits::quiet_NaN();
@@ -109,7 +121,6 @@ Orbita2dSystem::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
hw_states_temperature_[i] = std::numeric_limits::quiet_NaN();
hw_states_torque_limit_[i] = std::numeric_limits::quiet_NaN();
hw_states_speed_limit_[i] = std::numeric_limits::quiet_NaN();
- hw_states_torque_[i] = std::numeric_limits::quiet_NaN();
hw_states_p_gain_[i] = std::numeric_limits::quiet_NaN();
hw_states_i_gain_[i] = std::numeric_limits::quiet_NaN();
hw_states_d_gain_[i] = std::numeric_limits::quiet_NaN();
@@ -133,12 +144,8 @@ Orbita2dSystem::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
);
ret= CallbackReturn::ERROR;
}
- hw_commands_torque_[0] = torque_on;
- hw_commands_torque_[1] = torque_on;
-
- hw_states_torque_[0] = torque_on;
- hw_states_torque_[1] = torque_on;
-
+ hw_commands_torque_ = torque_on ? 1.0 : 0.0;
+ hw_states_torque_ = torque_on ? 1.0 : 0.0;
//init the states to the read values
@@ -220,11 +227,12 @@ Orbita2dSystem::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
}
//set the commands to the read values (otherwise some strange behaviour can happen)
+ hw_commands_torque_ = hw_states_torque_;
+
for (int i=0; i < 2; i++) {
hw_commands_position_[i]=hw_states_position_[i];
hw_commands_torque_limit_[i]=hw_states_torque_limit_[i];
hw_commands_speed_limit_[i]=hw_states_speed_limit_[i];
- hw_commands_torque_[i]=hw_states_torque_[i];
hw_commands_p_gain_[i]=hw_states_p_gain_[i];
hw_commands_i_gain_[i]=hw_states_i_gain_[i];
hw_commands_d_gain_[i]=hw_states_d_gain_[i];
@@ -268,25 +276,44 @@ Orbita2dSystem::export_state_interfaces()
joint.name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocity_[i]));
state_interfaces.emplace_back(hardware_interface::StateInterface(
joint.name, hardware_interface::HW_IF_EFFORT, &hw_states_effort_[i]));
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- joint.name, "temperature", &hw_states_temperature_[i]));
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- joint.name, "torque_limit", &hw_states_torque_limit_[i]));
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- joint.name, "speed_limit", &hw_states_speed_limit_[i]));
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- joint.name, "torque", &hw_states_torque_[i]));
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- joint.name, "p_gain", &hw_states_p_gain_[i]));
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- joint.name, "i_gain", &hw_states_i_gain_[i]));
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- joint.name, "d_gain", &hw_states_d_gain_[i]));
+ }
- RCLCPP_INFO(
- rclcpp::get_logger("Orbita2dSystem"),
- "export state interface (%s) \"%s\"!", info_.name.c_str(), joint.name.c_str()
+ for (std::size_t i = 0; i < 3; i++)
+ {
+ auto gpio = info_.gpios[i];
+
+ if (gpio.name == info_.name.c_str()) {
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ info_.name.c_str(), "torque", &hw_states_torque_));
+
+ RCLCPP_INFO(
+ rclcpp::get_logger("Orbita2dSystem"),
+ "export state interface (%s) \"%s\"!", info_.name.c_str(), gpio.name.c_str()
);
+ } else if (gpio.name.find("raw_motor") != std::string::npos) {
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ gpio.name, "temperature", &hw_states_temperature_[i]));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ gpio.name, "torque_limit", &hw_states_torque_limit_[i]));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ gpio.name, "speed_limit", &hw_states_speed_limit_[i]));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ gpio.name, "p_gain", &hw_states_p_gain_[i]));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ gpio.name, "i_gain", &hw_states_i_gain_[i]));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ gpio.name, "d_gain", &hw_states_d_gain_[i]));
+
+ RCLCPP_INFO(
+ rclcpp::get_logger("Orbita2dSystem"),
+ "export state interface (%s) \"%s\"!", info_.name.c_str(), gpio.name.c_str()
+ );
+ } else {
+ RCLCPP_WARN(
+ rclcpp::get_logger("Orbita2dSystem"),
+ "Unkwon state interface (%s) \"%s\"!", info_.name.c_str(), gpio.name.c_str()
+ );
+ }
}
return state_interfaces;
@@ -303,23 +330,42 @@ Orbita2dSystem::export_command_interfaces()
command_interfaces.emplace_back(hardware_interface::CommandInterface(
joint.name, hardware_interface::HW_IF_POSITION, &hw_commands_position_[i]));
- command_interfaces.emplace_back(hardware_interface::CommandInterface(
- joint.name, "speed_limit", &hw_commands_speed_limit_[i]));
- command_interfaces.emplace_back(hardware_interface::CommandInterface(
- joint.name, "torque_limit", &hw_commands_torque_limit_[i]));
- command_interfaces.emplace_back(hardware_interface::CommandInterface(
- joint.name, "torque", &hw_commands_torque_[i]));
- command_interfaces.emplace_back(hardware_interface::CommandInterface(
- joint.name, "p_gain", &hw_commands_p_gain_[i]));
- command_interfaces.emplace_back(hardware_interface::CommandInterface(
- joint.name, "i_gain", &hw_commands_i_gain_[i]));
- command_interfaces.emplace_back(hardware_interface::CommandInterface(
- joint.name, "d_gain", &hw_commands_d_gain_[i]));
+ }
- RCLCPP_INFO(
- rclcpp::get_logger("Orbita2dSystem"),
- "export command interface (%s) \"%s\"!", info_.name.c_str(), joint.name.c_str()
+ for (std::size_t i = 0; i < 3; i++)
+ {
+ auto gpio = info_.gpios[i];
+
+ if (gpio.name == info_.name.c_str()) {
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ info_.name.c_str(), "torque", &hw_commands_torque_));
+ RCLCPP_INFO(
+ rclcpp::get_logger("Orbita2dSystem"),
+ "export command interface (%s) \"%s\"!", info_.name.c_str(), gpio.name.c_str()
);
+
+ } else if (gpio.name.find("raw_motor") != std::string::npos) {
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ gpio.name, "speed_limit", &hw_commands_speed_limit_[i]));
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ gpio.name, "torque_limit", &hw_commands_torque_limit_[i]));
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ gpio.name, "p_gain", &hw_commands_p_gain_[i]));
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ gpio.name, "i_gain", &hw_commands_i_gain_[i]));
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ gpio.name, "d_gain", &hw_commands_d_gain_[i]));
+
+ RCLCPP_INFO(
+ rclcpp::get_logger("Orbita2dSystem"),
+ "export command interface (%s) \"%s\"!", info_.name.c_str(), gpio.name.c_str()
+ );
+ } else {
+ RCLCPP_WARN(
+ rclcpp::get_logger("Orbita2dSystem"),
+ "Unkown command interface (%s) \"%s\"!", info_.name.c_str(), gpio.name.c_str()
+ );
+ }
}
return command_interfaces;
@@ -355,8 +401,7 @@ Orbita2dSystem::read(const rclcpp::Time &, const rclcpp::Duration &)
"(%s) Error getting torque status!",info_.name.c_str()
);
}
- hw_states_torque_[0] = torque_on;
- hw_states_torque_[1] = torque_on;
+ hw_states_torque_ = torque_on ? 1.0 : 0.0;
// //Velocity
@@ -451,7 +496,7 @@ Orbita2dSystem::write(const rclcpp::Time &, const rclcpp::Duration &)
//We only change torque for both axes
- bool torque= (hw_commands_torque_[0]==1.0 && hw_commands_torque_[1]==1.0);
+ bool torque = (hw_commands_torque_ != 0.0);
//TODO: we can go with orbita2d_set_raw_torque_limit
if(torque)
{