Merge pull request #28 from pollen-robotics/support-poulpe

Support poulpe

.github/workflows/publish.yml

@@ -22,8 +22,8 @@ jobs:
 
     - uses: actions-rs/toolchain@v1
       with:
-          toolchain: stable
+          toolchain: nightly
           override: true
     - uses: katyo/publish-crates@v1
       with:
-          registry-token: ${{ secrets.CARGO_REGISTRY_TOKEN }}
+          registry-token: ${{ secrets.CARGO_REGISTRY_TOKEN }}

.github/workflows/rust.yml

@@ -12,6 +12,17 @@ jobs:
         steps:
         - uses: actions/checkout@v3
 
+        - name: Setup Rust Toolchain
+          uses: actions-rs/toolchain@v1
+          with:
+            toolchain: nightly
+            override: true
+
+        - name: Toolchain in nightly mode
+          run: |
+            rustup component add rustfmt --toolchain nightly
+            rustup component add clippy --toolchain nightly
+    
         - name: Check
           run: |
             cargo check --all --tests

src/fake_motor.rs

@@ -162,6 +162,27 @@ impl<const N: usize> RawMotorsIO<N> for FakeMotorsIO<N> {
         Ok(())
     }
 
+    fn set_target_position_fb(&mut self, target_position: [f64; N]) -> Result<[f64; N]> {
+        log::debug!(target: "fake_io::set_target_position", "Setting target_position to {:?}", target_position);
+        self.target_position = target_position;
+        let mut fb: [f64; N] = [0.0; { N }];
+
+        for (cur, on, target) in izip!(&mut self.current_position, self.torque_on, target_position)
+        {
+            if on {
+                log::debug!(target: "fake_io::set_target_position", "Setting current position to target position {:?} (torque on)", target);
+                *cur = target;
+            } else {
+                log::debug!(target: "fake_io::set_target_position", "Current position unchanged (torque off)");
+            }
+        }
+        fb[0..N].copy_from_slice(&self.current_position);
+        // fb[N..2*N].copy_from_slice(&self.current_velocity);
+        // fb[2*N..3*N].copy_from_slice(&self.current_torque);
+
+        Ok(fb)
+    }
+
     fn get_velocity_limit(&mut self) -> Result<[f64; N]> {
         Ok(self.velocity_limit)
     }
@@ -191,6 +212,17 @@ impl<const N: usize> RawMotorsIO<N> for FakeMotorsIO<N> {
         self.pid = pid;
         Ok(())
     }
+
+    fn get_axis_sensors(&mut self) -> Result<[f64; N]> {
+        Ok(self.current_position)
+    }
+
+    fn get_board_state(&mut self) -> Result<u8> {
+        Ok(0)
+    }
+    fn set_board_state(&mut self, _state: u8) -> Result<()> {
+        Ok(())
+    }
 }
 
 #[cfg(test)]

src/lib.rs

@@ -1,3 +1,6 @@
+#![feature(generic_const_exprs)]
+#![allow(incomplete_features)]
+
 mod fake_motor;
 pub use fake_motor::FakeMotorsController;
 

src/motors_controller.rs

@@ -35,7 +35,7 @@ pub trait MotorsController<const N: usize> {
                 position[i] -= offsets;
             }
         }
-        log::debug!(target: "controller::get_current_position", "after offset/reduction current_position: {:?}", position);
+        log::debug!(target: "controller::get_current_position", "after offset/reduction current_position: {:?} (reductions {:?} offsets {:?})", position,reductions,offsets);
 
         Ok(position)
     }
@@ -119,68 +119,111 @@ pub trait MotorsController<const N: usize> {
         self.io().set_target_position(limited_position)
     }
 
-    /// Get the velocity limit of the motors (in radians per second)
-    fn get_velocity_limit(&mut self) -> Result<[f64; N]> {
-        let mut velocity = self.io().get_velocity_limit()?;
-        log::debug!(target: "controller::get_velocity_limit", "raw velocity_limit: {:?}", velocity);
+    /// Set the current target position and returns the motor feeback (position, velocity, torque)
+    fn set_target_position_fb(&mut self, position: [f64; N]) -> Result<[f64; N]> {
+        log::debug!(target: "controller::set_target_position", "real target_position: {:?}", position);
+
+        let mut limited_position = position;
+        for i in 0..N {
+            if let Some(limits) = self.limits()[i] {
+                limited_position[i] = limits.clamp(position[i]);
+            }
+        }
 
         let reductions = self.reduction();
+        let offsets = self.offsets();
 
         for i in 0..N {
+            if let Some(offsets) = offsets[i] {
+                limited_position[i] += offsets;
+            }
             if let Some(reductions) = reductions[i] {
-                velocity[i] /= reductions;
+                limited_position[i] *= reductions;
             }
         }
-        log::debug!(target: "controller::get_velocity_limit", "after reduction velocity_limit: {:?}", velocity);
 
-        Ok(velocity)
-    }
-    /// Set the velocity limit of the motors (in radians per second)
-    fn set_velocity_limit(&mut self, velocity: [f64; N]) -> Result<()> {
-        log::debug!(target: "controller::set_velocity_limit", "real velocity_limit: {:?}", velocity);
+        log::debug!(target: "controller::set_target_position", "raw target_position: {:?}", limited_position);
 
-        let reductions = self.reduction();
-        let mut velocity = velocity;
+        let mut fb = self.io().set_target_position_fb(limited_position)?;
+        // let ret:[f64;N*3]=fb?;
+        // let ret=[0.0;N*3];
 
         for i in 0..N {
             if let Some(reductions) = reductions[i] {
-                velocity[i] *= reductions;
+                fb[i] /= reductions; //position
+                                     // fb[i + N] /= reductions; //velocity
+                                     // fb[i+N*2] /= reductions; //torque
+            }
+            if let Some(offsets) = offsets[i] {
+                fb[i] -= offsets;
             }
         }
-        log::debug!(target: "controller::set_velocity_limit", "raw velocity_limit: {:?}", velocity);
 
+        Ok(fb)
+    }
+
+    /// Get the velocity limit of the motors (in radians per second)
+    fn get_velocity_limit(&mut self) -> Result<[f64; N]> {
+        let velocity = self.io().get_velocity_limit()?;
+        log::debug!(target: "controller::get_velocity_limit", "raw velocity_limit: {:?}", velocity);
+        /*
+            let reductions = self.reduction();
+
+            for i in 0..N {
+                if let Some(reductions) = reductions[i] {
+                    velocity[i] /= reductions;
+                }
+            }
+            log::debug!(target: "controller::get_velocity_limit", "after reduction velocity_limit: {:?}", velocity);
+        */
+        Ok(velocity)
+    }
+    /// Set the velocity limit of the motors (in radians per second)
+    fn set_velocity_limit(&mut self, velocity: [f64; N]) -> Result<()> {
+        log::debug!(target: "controller::set_velocity_limit", "real velocity_limit: {:?}", velocity);
+        /*
+            let reductions = self.reduction();
+            let mut velocity = velocity;
+
+            for i in 0..N {
+                if let Some(reductions) = reductions[i] {
+                    velocity[i] *= reductions;
+                }
+            }
+            log::debug!(target: "controller::set_velocity_limit", "raw velocity_limit: {:?}", velocity);
+        */
         self.io().set_velocity_limit(velocity)
     }
     /// Get the torque limit of the motors (in Nm)
     fn get_torque_limit(&mut self) -> Result<[f64; N]> {
-        let mut torque = self.io().get_torque_limit()?;
+        let torque = self.io().get_torque_limit()?;
         log::debug!(target: "controller::get_torque_limit", "raw torque_limit: {:?}", torque);
+        /*
+            let reductions = self.reduction();
 
-        let reductions = self.reduction();
-
-        for i in 0..N {
-            if let Some(reductions) = reductions[i] {
-                torque[i] /= reductions;
+            for i in 0..N {
+                if let Some(reductions) = reductions[i] {
+                    torque[i] /= reductions;
+                }
             }
-        }
-        log::debug!(target: "controller::get_torque_limit", "after reduction torque_limit: {:?}", torque);
-
+            log::debug!(target: "controller::get_torque_limit", "after reduction torque_limit: {:?}", torque);
+        */
         Ok(torque)
     }
     /// Set the torque limit of the motors (in Nm)
     fn set_torque_limit(&mut self, torque: [f64; N]) -> Result<()> {
         log::debug!(target: "controller::set_torque_limit", "real torque_limit: {:?}", torque);
-
-        let reductions = self.reduction();
-        let mut torque = torque;
-
-        for i in 0..N {
-            if let Some(reductions) = reductions[i] {
-                torque[i] *= reductions;
+        /*
+            let reductions = self.reduction();
+            let mut torque = torque;
+
+            for i in 0..N {
+                if let Some(reductions) = reductions[i] {
+                    torque[i] *= reductions;
+                }
             }
-        }
-        log::debug!(target: "controller::set_torque_limit", "raw torque_limit: {:?}", torque);
-
+            log::debug!(target: "controller::set_torque_limit", "raw torque_limit: {:?}", torque);
+        */
         self.io().set_torque_limit(torque)
     }
     /// Get the current PID gains of the motors
@@ -191,6 +234,20 @@ pub trait MotorsController<const N: usize> {
     fn set_pid_gains(&mut self, pid: [PID; N]) -> Result<()> {
         self.io().set_pid_gains(pid)
     }
+
+    /// Get the current axis sensors of the articulation
+    fn get_axis_sensors(&mut self) -> Result<[f64; N]> {
+        self.io().get_axis_sensors()
+    }
+
+    /// Get the current state of the articulation control board
+    fn get_board_state(&mut self) -> Result<u8> {
+        self.io().get_board_state()
+    }
+    /// Set the current state of the articulation control board (clear error)
+    fn set_board_state(&mut self, state: u8) -> Result<()> {
+        self.io().set_board_state(state)
+    }
 }
 
 #[derive(Debug)]

src/motors_io.rs

@@ -18,6 +18,9 @@ pub trait RawMotorsIO<const N: usize> {
     /// Set the current target position of the motors (in radians)
     fn set_target_position(&mut self, position: [f64; N]) -> Result<()>;
 
+    /// Set the current target position and returns the motor feeback (position, velocity, torque)
+    fn set_target_position_fb(&mut self, position: [f64; N]) -> Result<[f64; N]>;
+
     /// Get the velocity limit of the motors (in radians per second)
     fn get_velocity_limit(&mut self) -> Result<[f64; N]>;
     /// Set the velocity limit of the motors (in radians per second)
@@ -32,4 +35,13 @@ pub trait RawMotorsIO<const N: usize> {
     fn get_pid_gains(&mut self) -> Result<[PID; N]>;
     /// Set the current PID gains of the motors
     fn set_pid_gains(&mut self, pid: [PID; N]) -> Result<()>;
+
+    /// Get the current axis sensors
+    fn get_axis_sensors(&mut self) -> Result<[f64; N]>;
+
+    /// Get the Board State byte
+    fn get_board_state(&mut self) -> Result<u8>;
+
+    /// Set the Board State byte
+    fn set_board_state(&mut self, state: u8) -> Result<()>;
 }