Merge branch 'andrew/optimization_controller' into andrew/metrics

ff_control/ff_control/wrench_ctrl.py

@@ -96,7 +96,7 @@ def set_world_wrench(self, wrench_world: Wrench2D, theta: float) -> None:
 
     def clip_wrench(self, wrench: Wrench2D) -> Wrench2D:
         wrench_clipped = Wrench2D()
-        force = np.sqrt(wrench.fx ** 2 + wrench.fy ** 2)
+        force = np.sqrt(wrench.fx**2 + wrench.fy**2)
         force_scale = max(force / self.p.actuators["F_body_max"], 1.0)
         torque_scale = max(abs(wrench.tz) / self.p.actuators["M_body_max"], 1.0)
 

ff_control/package.xml

@@ -17,6 +17,8 @@
   <depend>rclpy</depend>
   <depend>geometry_msgs</depend>
 
+  <exec_depend>casadi-pip</exec_depend>
+
   <depend>ff_estimate</depend>
   <depend>ff_msgs</depend>
   <depend>ff_params</depend>

ff_drivers/CMakeLists.txt

@@ -12,27 +12,27 @@ find_package(rclcpp REQUIRED)
 find_package(ff_msgs REQUIRED)
 find_package(ff_control REQUIRED)
 
-add_library(pwm src/pwm.cpp)
-target_include_directories(pwm PUBLIC
+add_library(driver_lib src/pwm.cpp src/gpio.cpp)
+target_include_directories(driver_lib PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
-target_compile_features(pwm PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
-ament_target_dependencies(pwm rclcpp)
+target_compile_features(driver_lib PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+ament_target_dependencies(driver_lib rclcpp)
 
-# nodes (TODO: needs to be updated)
-# add_executable(thruster_node src/thruster_node.cpp)
-# target_link_libraries(thruster_node pwm)
-# ament_target_dependencies(thruster_node ff_msgs)
+add_executable(thruster_node src/thruster_node.cpp)
+target_link_libraries(thruster_node driver_lib)
+ament_target_dependencies(thruster_node ff_msgs)
 
 # tests
+# TODO(alvin): remove pwm stuff
 add_executable(test_single src/tests/test_single.cpp)
-target_link_libraries(test_single pwm)
+target_link_libraries(test_single driver_lib)
 
 add_executable(test_all_thrusters src/tests/test_all_thrusters.cpp)
 ament_target_dependencies(test_all_thrusters rclcpp ff_msgs ff_control)
 
 # install nodes
-install(TARGETS test_single test_all_thrusters
+install(TARGETS thruster_node test_single test_all_thrusters
         DESTINATION lib/${PROJECT_NAME})
 
 # install launch files

ff_drivers/include/ff_drivers/gpio.hpp

@@ -0,0 +1,51 @@
+// MIT License
+//
+// Copyright (c) 2024 Stanford Autonomous Systems Lab
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+
+#pragma once
+
+#include <fstream>
+#include <string>
+
+namespace ff
+{
+
+class GPIO
+{
+public:
+  explicit GPIO(int pin);
+  ~GPIO();
+
+  void SetState(bool state);
+  void SetPolarity(bool normal = true);
+  bool GetState() const;
+
+private:
+  int pin_;
+  bool state_;
+  std::ofstream f_value_;
+  std::ofstream f_active_low_;
+
+  std::string BasePath() const;
+};
+
+}  // namespace ff

ff_drivers/launch/hardware_bringup.launch.py

@@ -53,7 +53,7 @@ def generate_launch_description():
                 # ASL optitrack IP and port
                 parameters=[
                     {
-                        "server": "192.168.1.8",
+                        "server": "192.168.1.2",
                         "port": 3883,
                     }
                 ],

ff_drivers/src/gpio.cpp

@@ -0,0 +1,87 @@
+// MIT License
+//
+// Copyright (c) 2024 Stanford Autonomous Systems Lab
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+
+#include "ff_drivers/gpio.hpp"
+
+namespace ff
+{
+
+GPIO::GPIO(int pin)
+: pin_(pin)
+{
+  // request gpio device
+  {
+    std::ofstream f_export("/sys/class/gpio/export");
+    f_export << pin;
+  }
+
+  // configure GPIO to be an output
+  {
+    std::ofstream f_direction(BasePath() + "direction");
+    f_direction << "out";
+  }
+
+  f_value_.open(BasePath() + "value");
+  f_active_low_.open(BasePath() + "active_low");
+
+  SetPolarity(true);
+  SetState(false);
+}
+
+GPIO::~GPIO()
+{
+  SetState(false);
+
+  // close files
+  f_value_.close();
+  f_active_low_.close();
+
+  // free gpio device
+  std::ofstream f_unexport("/sys/class/gpio/unexport");
+  f_unexport << pin_;
+}
+
+void GPIO::SetState(bool state)
+{
+  f_value_ << static_cast<int>(state);
+  f_value_.flush();
+  state_ = state;
+}
+
+bool GPIO::GetState() const
+{
+  return state_;
+}
+
+void GPIO::SetPolarity(bool normal)
+{
+  f_active_low_ << static_cast<int>(!normal);
+  f_active_low_.flush();
+}
+
+std::string GPIO::BasePath() const
+{
+  return "/sys/class/gpio/gpio" + std::to_string(pin_) + "/";
+}
+
+}  // namespace ff

ff_drivers/src/thruster_node.cpp

@@ -1,6 +1,6 @@
 // MIT License
 //
-// Copyright (c) 2023 Stanford Autonomous Systems Lab
+// Copyright (c) 2024 Stanford Autonomous Systems Lab
 //
 // Permission is hereby granted, free of charge, to any person obtaining a copy
 // of this software and associated documentation files (the "Software"), to deal
@@ -21,13 +21,12 @@
 // SOFTWARE.
 
 
-#include <chrono>
 #include <memory>
-
+#include <vector>
 #include <rclcpp/rclcpp.hpp>
 
-#include "ff_drivers/pwm.hpp"
-#include "ff_msgs/msg/thruster_pwm_command.hpp"
+#include "ff_drivers/gpio.hpp"
+#include "ff_msgs/msg/thruster_command.hpp"
 
 #define NUM_THRUSTERS 8
 
@@ -45,47 +44,36 @@ static constexpr int THRUSTER_PINS[NUM_THRUSTERS] = {
 };
 
 
-using namespace std::chrono_literals;
-using ff_msgs::msg::ThrusterPWMCommand;
+using ff_msgs::msg::ThrusterCommand;
 
-class ThrusterNode : public ff::PWMManager
+class ThrusterNode : public rclcpp::Node
 {
 public:
   ThrusterNode()
-  : ff::PWMManager("thruster_driver_node")
+  : rclcpp::Node("thruster_node")
   {
-    // add all PWMs
+    // initialize all GPIO
     for (size_t i = 0; i < NUM_THRUSTERS; ++i) {
-      this->AddSoftPWM(THRUSTER_PINS[i]);
+      gpios_.push_back(std::make_unique<ff::GPIO>(THRUSTER_PINS[i]));
     }
 
-    // set period (default to 10Hz)
-    double period = this->declare_parameter("period", .1);
-    this->SetPeriodAll(period * 1s);
-    // update period on the fly
-    sub_params_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
-    cb_period_ = sub_params_->add_parameter_callback(
-      "period",
-      [this](const rclcpp::Parameter & p) {SetPeriodAll(p.as_double() * 1s);});
-
-    // start all PWMs
-    this->EnableAll();
-
     // listen to commands
-    sub_duty_cycle_ = this->create_subscription<ThrusterPWMCommand>(
-      "commands/duty_cycle",
-      10, [this](const ThrusterPWMCommand::SharedPtr msg) {DutyCycleCallback(msg);});
+    sub_thruster_ = this->create_subscription<ThrusterCommand>(
+      "commands/binary_thrust",
+      10,
+      [this](const ThrusterCommand::SharedPtr msg) {ThrusterCommandCallback(msg);});
   }
 
 private:
-  std::shared_ptr<rclcpp::ParameterEventHandler> sub_params_;
-  std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_period_;
-  rclcpp::Subscription<ThrusterPWMCommand>::SharedPtr sub_duty_cycle_;
+  std::vector<std::unique_ptr<ff::GPIO>> gpios_;
+  rclcpp::Subscription<ThrusterCommand>::SharedPtr sub_thruster_;
 
-  void DutyCycleCallback(const ThrusterPWMCommand::SharedPtr msg)
+  void ThrusterCommandCallback(const ThrusterCommand::SharedPtr msg)
   {
     for (size_t i = 0; i < NUM_THRUSTERS; ++i) {
-      this->SetDutyCycle(i, msg->duty_cycles[i]);
+      if (gpios_[i]->GetState() != msg->switches[i]) {
+        gpios_[i]->SetState(msg->switches[i]);
+      }
     }
   }
 };

ff_drivers/src/thruster_pwm_node.cpp

@@ -0,0 +1,99 @@
+// MIT License
+//
+// Copyright (c) 2023 Stanford Autonomous Systems Lab
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+
+#include <chrono>
+#include <memory>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "ff_drivers/pwm.hpp"
+#include "ff_msgs/msg/thruster_pwm_command.hpp"
+
+#define NUM_THRUSTERS 8
+
+// thruster pin connection
+// @see: https://wiki.odroid.com/odroid-n2l/application_note/gpio/pwm#tab__odroid-n2
+static constexpr int THRUSTER_PINS[NUM_THRUSTERS] = {
+  476,  // thruster pin 1 -> odroid pin 16
+  477,  // thruster pin 2 -> odroid pin 18
+  484,  // thruster pin 3 -> odroid pin 19
+  485,  // thruster pin 4 -> odroid pin 21
+  478,  // thruster pin 5 -> odroid pin 22
+  487,  // thruster pin 6 -> odroid pin 23
+  486,  // thruster pin 7 -> odroid pin 24
+  464,  // thruster pin 8 -> odroid pin 26
+};
+
+
+using namespace std::chrono_literals;
+using ff_msgs::msg::ThrusterPWMCommand;
+
+class ThrusterNode : public ff::PWMManager
+{
+public:
+  ThrusterNode()
+  : ff::PWMManager("thruster_driver_node")
+  {
+    // add all PWMs
+    for (size_t i = 0; i < NUM_THRUSTERS; ++i) {
+      this->AddSoftPWM(THRUSTER_PINS[i]);
+    }
+
+    // set period (default to 10Hz)
+    double period = this->declare_parameter("period", .1);
+    this->SetPeriodAll(period * 1s);
+    // update period on the fly
+    sub_params_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
+    cb_period_ = sub_params_->add_parameter_callback(
+      "period",
+      [this](const rclcpp::Parameter & p) {SetPeriodAll(p.as_double() * 1s);});
+
+    // start all PWMs
+    this->EnableAll();
+
+    // listen to commands
+    sub_duty_cycle_ = this->create_subscription<ThrusterPWMCommand>(
+      "commands/duty_cycle",
+      10, [this](const ThrusterPWMCommand::SharedPtr msg) {DutyCycleCallback(msg);});
+  }
+
+private:
+  std::shared_ptr<rclcpp::ParameterEventHandler> sub_params_;
+  std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_period_;
+  rclcpp::Subscription<ThrusterPWMCommand>::SharedPtr sub_duty_cycle_;
+
+  void DutyCycleCallback(const ThrusterPWMCommand::SharedPtr msg)
+  {
+    for (size_t i = 0; i < NUM_THRUSTERS; ++i) {
+      this->SetDutyCycle(i, msg->duty_cycles[i]);
+    }
+  }
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<ThrusterNode>());
+  rclcpp::shutdown();
+  return 0;
+}