pd plus demo node, WIP

CMakeLists.txt

@@ -29,20 +29,33 @@ sec_generate_parameter_library(
   src/linear_feedback_controller.yaml # Path to input yaml file
 )
 add_dependencies(${PROJECT_NAME}_parameters ${PROJECT_NAME}_parameters_doc)
+# Joint estimator params
+sec_generate_parameter_library_markdown(joint_state_estimator_parameters_doc
+                                        src/linear_feedback_controller.yaml)
+sec_generate_parameter_library(
+  joint_state_estimator_generated_parameters # Lib name
+  joint_state_estimator_parameters # CMake target name for the parameter
+                                   # library.
+  src/joint_state_estimator.yaml # Path to input yaml file
+)
+add_dependencies(joint_state_estimator_parameters
+                 joint_state_estimator_parameters_doc)
 
 #
 # Main Library
 #
 set(${PROJECT_NAME}_headers
     include/${PROJECT_NAME}/averaging_filter.hpp
     include/${PROJECT_NAME}/averaging_filter.hxx
+    include/${PROJECT_NAME}/joint_state_estimator.hpp
     include/${PROJECT_NAME}/lf_controller.hpp
     include/${PROJECT_NAME}/linear_feedback_controller.hpp
     include/${PROJECT_NAME}/linear_feedback_controller_ros.hpp
     include/${PROJECT_NAME}/min_jerk.hpp
     include/${PROJECT_NAME}/pd_controller.hpp
     include/${PROJECT_NAME}/robot_model_builder.hpp)
 set(${PROJECT_NAME}_sources
+    src/joint_state_estimator.cpp #
     src/lf_controller.cpp #
     src/linear_feedback_controller.cpp #
     src/linear_feedback_controller_ros.cpp #
@@ -52,7 +65,7 @@ set(${PROJECT_NAME}_sources
 ament_auto_add_library(${PROJECT_NAME} ${${PROJECT_NAME}_sources}
                        ${${PROJECT_NAME}_headers})
 ament_target_dependencies(${PROJECT_NAME} Eigen3)
-ament_target_dependencies(${PROJECT_NAME} pinocchio)
+target_link_libraries(${PROJECT_NAME} pinocchio::pinocchio)
 target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_parameters)
 
 #
@@ -62,29 +75,17 @@ include(CTest)
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_auto_find_test_dependencies()
-  # Integration test of the roscontrol controller with simulation on Talos.
-  # add_rostest(tests/test_linear_feedback_controller.test)
 
-  # Loading test, especially the parameters add_rostest_gtest(
-  # test_linear_feedback_controller_basic
-  # tests/test_linear_feedback_controller_basic.test
-  # tests/test_linear_feedback_controller_basic.cpp tests/ros_gtest_main.cpp)
-  # target_link_libraries(test_linear_feedback_controller_basic ${PROJECT_NAME}
-  # example-robot-data::example-robot-data) target_compile_definitions(
-  # test_linear_feedback_controller_basic PRIVATE
-  # TEST_ROS_NODE_NAME="test_linear_feedback_controller_basic")
-
-  # Test the Averaging filter class.
   ament_auto_add_gtest(test_averaging_filter tests/test_averaging_filter.cpp)
-  target_link_libraries(test_averaging_filter gtest gtest_main ${PROJECT_NAME})
+  target_link_libraries(test_averaging_filter ${PROJECT_NAME})
 
   ament_auto_add_gtest(test_min_jerk tests/test_min_jerk.cpp)
-  target_link_libraries(test_min_jerk gtest gtest_main ${PROJECT_NAME})
+  target_link_libraries(test_min_jerk ${PROJECT_NAME})
 
   ament_auto_add_gtest(test_robot_model_builder
                        tests/test_robot_model_builder.cpp)
-  target_link_libraries(test_robot_model_builder gtest gtest_main
-                        ${PROJECT_NAME} example-robot-data::example-robot-data)
+  target_link_libraries(test_robot_model_builder ${PROJECT_NAME}
+                        example-robot-data::example-robot-data)
 endif()
 
 #
@@ -96,11 +97,13 @@ pluginlib_export_plugin_description_file(controller_interface
 #
 # Installation
 #
+install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/doc
+        DESTINATION share/${PROJECT_NAME})
 install(
-  PROGRAMS src/pd_controller.py
+  PROGRAMS tests/pd_plus_controller.py
   DESTINATION lib/${PROJECT_NAME}
-  RENAME pd_controller)
+  RENAME pd_plus_controller)
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
-
+ament_export_libraries(${PROJECT_NAME})
 ament_auto_package()

cmake/sec_generate_parameter_library.cmake

@@ -111,8 +111,7 @@ function(sec_generate_parameter_library_markdown TARGET_NAME YAML_FILE)
 
   # Set the output parameter header file name
   get_filename_component(MARKDOWN_FILE ${YAML_FILE} NAME_WE)
-  set(MARKDOWN_FILE
-      ${CMAKE_CURRENT_BINARY_DIR}/doc/${MARKDOWN_FILE}/${MARKDOWN_FILE}.md)
+  set(MARKDOWN_FILE ${CMAKE_CURRENT_BINARY_DIR}/doc/${MARKDOWN_FILE}.md)
 
   # Generate the header for the library
   add_custom_target(

controller_plugins.xml

@@ -1,11 +1,19 @@
-<library path="linear_feedback_controller_plugins">
+<library path="linear_feedback_controller">
 
-  <class name="linear_feedback_controller::LinearFeedbackController"
-         type="linear_feedback_controller::LinearFeedbackController"
-         base_class_type="controller_interface::ControllerBase">
+  <class name="linear_feedback_controller/LinearFeedbackController"
+         type="linear_feedback_controller::LinearFeedbackControllerRos"
+         base_class_type="controller_interface::ChainableControllerInterface">
     <description>
       Linear feedback controller with ROS topic based feedback loop.
     </description>
   </class>
 
+  <class name="linear_feedback_controller/JointStateEstimator"
+         type="linear_feedback_controller::JointStateEstimator"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>
+      Forward the joint state into the command potentially filtering some signals.
+    </description>
+  </class>
+
 </library>

include/linear_feedback_controller/joint_state_estimator.hpp

@@ -0,0 +1,55 @@
+#ifndef LINEAR_FEEDBACK_CONTROLLER__JOINT_STATE_ESTIMATOR_HPP
+#define LINEAR_FEEDBACK_CONTROLLER__JOINT_STATE_ESTIMATOR_HPP
+
+// system
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "std_msgs/msg/float64_multi_array.hpp"
+// auto-generated by generate_parameter_library
+#include "linear_feedback_controller/joint_state_estimator_generated_parameters.hpp"
+
+/**
+ * JointStateEstimator is a simple controller that exposes state interfaces
+ * equal to the number of it's command interfaces. This controller simply
+ * forwards the information from it's state interface to it's own command
+ * interfaces. It may filter some of the states.
+ */
+namespace linear_feedback_controller {
+using DataType = std_msgs::msg::Float64MultiArray;
+class JointStateEstimator : public controller_interface::ControllerInterface {
+ public:
+  controller_interface::CallbackReturn on_init() final;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration()
+      const final;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration()
+      const final;
+
+  controller_interface::CallbackReturn on_configure(
+      const rclcpp_lifecycle::State& previous_state) final;
+
+  controller_interface::CallbackReturn on_activate(
+      const rclcpp_lifecycle::State& previous_state) final;
+
+  controller_interface::CallbackReturn on_deactivate(
+      const rclcpp_lifecycle::State& previous_state) final;
+
+  controller_interface::return_type update(const rclcpp::Time& time,
+                                           const rclcpp::Duration& period);
+
+ protected:
+  std::shared_ptr<joint_state_estimator::ParamListener> param_listener_;
+  joint_state_estimator::Params params_;
+
+  std::vector<std::string> state_interface_names_;
+  std::vector<std::string> command_interface_names_;
+};
+}  // namespace linear_feedback_controller
+
+#endif  // LINEAR_FEEDBACK_CONTROLLER__JOINT_STATE_ESTIMATOR_HPP

include/linear_feedback_controller/linear_feedback_controller.hpp

@@ -7,6 +7,7 @@
 #include <vector>
 
 #include "Eigen/Core"
+#include "pinocchio/algorithm/rnea.hpp"
 
 // local include
 #include "linear_feedback_controller/lf_controller.hpp"
@@ -19,12 +20,10 @@ namespace linear_feedback_controller {
 
 struct ControllerParameters {
   std::string urdf;
-  std::string srdf;
   std::vector<std::string> moving_joint_names;
   std::vector<double> p_gains;
   std::vector<double> d_gains;
   std::vector<std::string> controlled_joint_names;
-  std::string default_configuration_name;
   bool robot_has_free_flyer;
   Duration pd_to_lf_transition_duration;
 };
@@ -63,8 +62,8 @@ class LinearFeedbackController {
 
   bool load(const ControllerParameters& params);
 
-  bool set_initial_state(const Eigen::VectorXd& tau_init,
-                         const Eigen::VectorXd& jq_init);
+  bool set_initial_state(Eigen::VectorXd const& tau_init,
+                         Eigen::VectorXd const& jq_init);
 
   /**
    * @brief
@@ -74,9 +73,9 @@ class LinearFeedbackController {
    * @param control
    * @return const Eigen::VectorXd&
    */
-  const Eigen::VectorXd& compute_control(const TimePoint& time,
-                                         const Sensor& sensor,
-                                         const Control& control);
+  const Eigen::VectorXd& compute_control(
+      const TimePoint& time, const Sensor& sensor, const Control& control,
+      const bool remove_gravity_compensation_effort);
 
   RobotModelBuilder::ConstSharedPtr get_robot_model() const;
 
@@ -95,6 +94,12 @@ class LinearFeedbackController {
   MinJerk min_jerk_;
   /// @brief Time at which we received the first control.
   TimePoint first_control_received_time_;
+  /// @brief Robot generalized coordinates.
+  Eigen::VectorXd robot_configuration_;
+  /// @brief Robot generalized velocity coordinates.
+  Eigen::VectorXd robot_velocity_;
+  /// @brief Robot generalized tangent space 0 coordinates.
+  Eigen::VectorXd robot_velocity_null_;
 };
 
 }  // namespace linear_feedback_controller

include/linear_feedback_controller/linear_feedback_controller_ros.hpp

@@ -5,11 +5,13 @@
 #include <memory>
 
 // ROS 2
+#include "control_toolbox/filters.hpp"
 #include "linear_feedback_controller_msgs/msg/control.hpp"
 #include "linear_feedback_controller_msgs/msg/sensor.hpp"
 #include "message_filters/subscriber.h"
 #include "message_filters/time_synchronizer.h"
 #include "nav_msgs/msg/odometry.hpp"
+#include "pluginlib/class_list_macros.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "realtime_tools/realtime_buffer.h"
 #include "sensor_msgs/msg/joint_state.hpp"
@@ -78,54 +80,54 @@ class LinearFeedbackControllerRos : public ChainableControllerInterface {
 
   /// @brief @copydoc ControllerInterfaceBase::command_interface_configuration
   /// This function access the output torques.
-  virtual InterfaceConfiguration command_interface_configuration() const final;
+  InterfaceConfiguration command_interface_configuration() const final;
 
   /// @brief @copydoc ControllerInterfaceBase::state_interface_configuration
   /// This function access none of the states.
-  virtual InterfaceConfiguration state_interface_configuration() const final;
+  InterfaceConfiguration state_interface_configuration() const final;
 
   /// @brief @copydoc
   /// ChainableControllerInterface::on_export_reference_interfaces This function
   /// access the reference state from an estimator.
-  virtual std::vector<hardware_interface::CommandInterface>
+  std::vector<hardware_interface::CommandInterface>
   on_export_reference_interfaces() final;
 
   /// @brief ChainableControllerInterface::update_reference_from_subscribers
-  virtual return_type update_reference_from_subscribers() final;
+  return_type update_reference_from_subscribers() final;
 
   /// @brief ChainableControllerInterface::update_and_write_commands
-  virtual return_type update_and_write_commands(
-      const rclcpp::Time& time, const rclcpp::Duration& period) final;
+  return_type update_and_write_commands(const rclcpp::Time& time,
+                                        const rclcpp::Duration& period) final;
 
   /// @brief @copydoc rclcpp_lifecycle::on_configure
-  virtual CallbackReturn on_configure(
+  CallbackReturn on_configure(
       const rclcpp_lifecycle::State& previous_state) final;
 
   /// @brief @copydoc ControllerInterfaceBase::on_init
   /// can return SUCCESS, FAILURE, or ERROR
-  virtual CallbackReturn on_init() final;
+  CallbackReturn on_init() final;
 
   /// @brief @copydoc rclcpp_lifecycle::on_activate
   ///
   /// Here we assume that the robot is not moving and has it's feet on the
   /// ground.
-  virtual CallbackReturn on_activate(
+  CallbackReturn on_activate(
       const rclcpp_lifecycle::State& previous_state) final;
 
   /// @brief @copydoc rclcpp_lifecycle::on_set_chained_mode
-  virtual bool on_set_chained_mode(bool chained_mode) final;
+  bool on_set_chained_mode(bool chained_mode) final;
 
   /// @brief @copydoc rclcpp_lifecycle::on_deactivate
-  virtual CallbackReturn on_deactivate(
+  CallbackReturn on_deactivate(
       const rclcpp_lifecycle::State& previous_state) final;
 
   /// @brief @copydoc rclcpp_lifecycle::on_cleanup
-  virtual controller_interface::CallbackReturn on_cleanup(
-      const rclcpp_lifecycle::State& previous_state) override;
+  controller_interface::CallbackReturn on_cleanup(
+      const rclcpp_lifecycle::State& previous_state) final;
 
   /// @brief @copydoc rclcpp_lifecycle::on_error
-  virtual controller_interface::CallbackReturn on_error(
-      const rclcpp_lifecycle::State& previous_state) override;
+  controller_interface::CallbackReturn on_error(
+      const rclcpp_lifecycle::State& previous_state) final;
 
  protected:
   // Initialization methods.
@@ -190,12 +192,10 @@ class LinearFeedbackControllerRos : public ChainableControllerInterface {
 
   // Reference interfaces.
   std::vector<std::string> reference_interface_names_;
-  std::string reference_prefix_;
 
   // Command interfaces.
   InterfaceVector<hardware_interface::LoanedCommandInterface>
       joint_effort_command_interface_;
-  std::string command_prefix_;
 
   /// @brief Controller without ROS.
   LinearFeedbackController lfc_;
@@ -229,6 +229,11 @@ class LinearFeedbackControllerRos : public ChainableControllerInterface {
   rclcpp::Publisher<SensorMsg>::SharedPtr sensor_publisher_;
   rclcpp::Subscription<ControlMsg>::SharedPtr control_subscriber_;
   ProtectedControlMsg synched_input_control_msg_;
+
+  // Used in the filtering of the joint velocity.
+  Eigen::VectorXd new_joint_velocity_;
+
+  bool first_time_update_and_write_commands_;
 };
 
 }  // namespace linear_feedback_controller