Merge pull request #75 from pollen-robotics/develop

update

Cargo.toml

@@ -1,4 +1,5 @@
 [workspace]
+resolver = "2"
 
 members = [
     "orbita2d_c_api",

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.
+   <details markdown="1">
+    <summary>If its your first time using rust with ros</summary>
+   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
+   ```
+
+   </details>
 * 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

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"

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<dyn std::error::Error>> {
+    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<dyn std::error::Error>> {
+    orbita2d.set_target_orientation([0.0, 0.0])?;
+    orbita2d.enable_torque(false)?;
+
+    Ok(())
+}
+
+fn controller_hwi_iteration(
+    orbita2d: &mut orbita2d_controller::Orbita2dController,
+) -> Result<(), Box<dyn std::error::Error>> {
+    controller_hwi_read_iteration(orbita2d)?;
+    controller_hwi_write_iteration(orbita2d)?;
+
+    Ok(())
+}
+
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    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(())
+}

orbita2d_description/ros2_control/orbita2d.ros2_control.xacro

@@ -1,7 +1,10 @@
 <?xml version="1.0" ?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:include filename="$(find pollen_generic_description)/ros2_control/joint.ros2_control.xacro"/>
+  <xacro:include filename="$(find orbita2d_description)/ros2_control/orbita2d_actuator_control.ros2_control.xacro"/>
+  <xacro:include filename="$(find orbita2d_description)/ros2_control/orbita2d_axis_control.ros2_control.xacro"/>
+  <xacro:include filename="$(find orbita2d_description)/ros2_control/orbita2d_raw_motor_control.ros2_control.xacro"/>
+
 
   <xacro:macro name="orbita2d_control" params="name joint1 joint2 config_file ">
 
@@ -25,8 +28,14 @@
           </xacro:unless>
         </xacro:if>
       </hardware>
-      <xacro:position_joint_control name="${name}_${joint1}"/>
-      <xacro:position_joint_control name="${name}_${joint2}"/>
+
+      <xacro:orbita2d_actuator_control name="${name}"/>
+
+      <xacro:orbita2d_axis_control name="${name}_${joint1}"/>
+      <xacro:orbita2d_axis_control name="${name}_${joint2}"/>
+
+      <xacro:orbita2d_raw_motor_control name="${name}_raw_motor_1"/>
+      <xacro:orbita2d_raw_motor_control name="${name}_raw_motor_2"/>
 
     </ros2_control>
 

orbita2d_description/ros2_control/orbita2d_actuator_control.ros2_control.xacro

@@ -0,0 +1,15 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="orbita2d_actuator_control" params="name">
+
+    <gpio name="${name}">
+      <state_interface name="torque">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+
+      <command_interface name="torque"/>
+
+    </gpio>
+
+  </xacro:macro>
+</robot>

orbita2d_description/ros2_control/orbita2d_axis_control.ros2_control.xacro

@@ -0,0 +1,18 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="orbita2d_axis_control" params="name">
+    <joint name="${name}">
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="effort">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+
+      <command_interface name="position"/>
+    </joint>
+  </xacro:macro>
+</robot>

orbita2d_description/ros2_control/orbita2d_raw_motor_control.ros2_control.xacro

@@ -0,0 +1,24 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="orbita2d_raw_motor_control" params="name">
+      <gpio name="${name}">
+        <state_interface name="speed_limit"/>
+        <command_interface name="speed_limit"/>
+
+        <state_interface name="torque_limit"/>
+        <command_interface name="torque_limit"/>
+
+        <state_interface name="p_gain"/>   
+        <command_interface name="p_gain"/>
+        <state_interface name="i_gain"/>   
+        <command_interface name="i_gain"/>
+        <state_interface name="d_gain"/>   
+        <command_interface name="d_gain"/>
+      </gpio>
+
+  </xacro:macro>
+</robot>
+
+
+
+

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];