Merge pull request #42 from MedericFourmy/humble-devel-mf

pd plus demo node, WIP

CMakeLists.txt

@@ -75,19 +75,7 @@ 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 ${PROJECT_NAME})
 
@@ -112,9 +100,9 @@ pluginlib_export_plugin_description_file(controller_interface
 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})

include/linear_feedback_controller/joint_state_estimator.hpp

@@ -1,17 +1,3 @@
-// Copyright (c) 2023, PAL Robotics
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
 #ifndef LINEAR_FEEDBACK_CONTROLLER__JOINT_STATE_ESTIMATOR_HPP
 #define LINEAR_FEEDBACK_CONTROLLER__JOINT_STATE_ESTIMATOR_HPP
 

package.xml

@@ -14,7 +14,7 @@
   <url>https://github.com/loco-3d/linear_feedback_controller</url>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
-
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <build_depend>eigen</build_depend>
   <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>

src/joint_state_estimator.cpp

@@ -1,17 +1,3 @@
-// Copyright (c) 2023, PAL Robotics
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
 #include "linear_feedback_controller/joint_state_estimator.hpp"
 
 #include "controller_interface/helpers.hpp"

src/lf_controller.cpp

@@ -10,16 +10,17 @@ LFController::~LFController() {}
 
 void LFController::initialize(const RobotModelBuilder::SharedPtr& rmb) {
   rmb_ = rmb;
-
-  desired_configuration_ = Eigen::VectorXd::Zero(rmb_->get_model().nq);
-  desired_velocity_ = Eigen::VectorXd::Zero(rmb_->get_model().nv);
-  measured_configuration_ = Eigen::VectorXd::Zero(rmb_->get_model().nq);
-  measured_velocity_ = Eigen::VectorXd::Zero(rmb_->get_model().nv);
-  if (rmb_->get_robot_has_free_flyer()) {
-    control_ = Eigen::VectorXd::Zero(rmb_->get_model().nv - kNbFreeFlyerDof);
-  } else {
-    control_ = Eigen::VectorXd::Zero(rmb_->get_model().nv);
-  }
+  const auto nq = rmb_->get_nq();
+  const auto nv = rmb_->get_nv();
+  const auto joint_nq = rmb_->get_joint_nq();
+  const auto joint_nv = rmb_->get_joint_nv();
+
+  desired_configuration_ = Eigen::VectorXd::Zero(nq);
+  desired_velocity_ = Eigen::VectorXd::Zero(nv);
+  measured_configuration_ = Eigen::VectorXd::Zero(nq);
+  measured_velocity_ = Eigen::VectorXd::Zero(nv);
+  control_ = Eigen::VectorXd::Zero(joint_nv);
+  diff_state_ = Eigen::VectorXd::Zero(2 * nv);
 }
 
 const Eigen::VectorXd& LFController::compute_control(

src/linear_feedback_controller.cpp

@@ -50,7 +50,7 @@ const Eigen::VectorXd& LinearFeedbackController::compute_control(
   const auto& ctrl_js = control.initial_state.joint_state;
 
   // Self documented variables.
-  const bool control_msg_received = !ctrl_js.name.empty();
+  const bool control_msg_received = !control.feedforward.hasNaN();
   const bool first_control_received_time_initialized =
       first_control_received_time_ != TimePoint::min();
   const bool during_switch = (time - first_control_received_time_) <

src/linear_feedback_controller_ros.cpp

@@ -1,8 +1,3 @@
-
-#define EIGEN_NO_MALLOC
-#define EIGEN_INTERNAL_DEBUGGING
-#define EIGEN_INITIALIZE_MATRICES_BY_NAN
-#define EIGEN_NO_AUTOMATIC_RESIZING
 #include "linear_feedback_controller/linear_feedback_controller_ros.hpp"
 
 #include "ament_index_cpp/get_package_share_directory.hpp"
@@ -327,6 +322,8 @@ bool LinearFeedbackControllerRos::read_state_from_references() {
         Eigen::VectorXd::Map(&reference_interfaces_[nq], 6);
     new_joint_velocity_ =
         Eigen::VectorXd::Map(&reference_interfaces_[nq + 6], joint_nv);
+    input_sensor_.joint_state.effort =
+        Eigen::VectorXd::Map(&reference_interfaces_[nq + nv], joint_nv);
   } else {
     input_sensor_.base_pose.fill(std::numeric_limits<double>::signaling_NaN());
     input_sensor_.joint_state.position =
@@ -340,8 +337,12 @@ bool LinearFeedbackControllerRos::read_state_from_references() {
         new_joint_velocity_[i], input_sensor_.joint_state.velocity[i],
         parameters_.joint_velocity_filter_coefficient);
   }
-  input_sensor_.joint_state.effort.fill(
-      std::numeric_limits<double>::signaling_NaN());
+
+  for (Eigen::Index i = 0; i < joint_nv; ++i) {
+    input_sensor_.joint_state.velocity(i) = filters::exponentialSmoothing(
+        new_joint_velocity_(i), input_sensor_.joint_state.velocity(i),
+        parameters_.joint_velocity_filter_coefficient);
+  }
   return true;
 }
 
@@ -546,6 +547,11 @@ bool LinearFeedbackControllerRos::setup_reference_interface() {
                       joint + "/" + HW_IF_VELOCITY;
     reference_interface_names_.emplace_back(name);
   }
+  for (const auto& joint : lfc_.get_robot_model()->get_moving_joint_names()) {
+    const auto name = parameters_.chainable_controller.reference_prefix +
+                      joint + "/" + HW_IF_EFFORT;
+    reference_interface_names_.emplace_back(name);
+  }
   return true;
 }
 
@@ -576,6 +582,13 @@ bool LinearFeedbackControllerRos::allocate_memory() {
   input_sensor_msg_.joint_state.velocity.resize(joint_nv, 0.0);
   input_sensor_msg_.joint_state.effort.resize(joint_nv, 0.0);
 
+  input_control_.initial_state = input_sensor_;
+  input_control_.feedback_gain = Eigen::MatrixXd::Zero(nv, 2 * nv);
+  input_control_.feedback_gain.fill(
+      std::numeric_limits<double>::signaling_NaN());
+  input_control_.feedforward = Eigen::VectorXd::Zero(nv);
+  input_control_.feedforward.fill(std::numeric_limits<double>::signaling_NaN());
+
   init_joint_position_ = Eigen::VectorXd::Zero(joint_nq);
   init_joint_effort_ = Eigen::VectorXd::Zero(joint_nv);
 
@@ -586,29 +599,24 @@ bool LinearFeedbackControllerRos::allocate_memory() {
   reference_interfaces_.resize(reference_interface_names_.size(), 0.0);
 
   // Allocate subscribers
-  {
-    rclcpp::QoS qos = rclcpp::QoS(10);
-    auto rmw_qos_profile = qos.get_rmw_qos_profile();
-    if (lfc_.get_robot_model()->get_robot_has_free_flyer()) {
-      subscriber_odom_.subscribe(get_node(), "odom", rmw_qos_profile);
-    }
-    subscriber_joint_state_.subscribe(get_node(), "joint_state",
-                                      rmw_qos_profile);
-    state_syncher_ = std::make_shared<
-        message_filters::TimeSynchronizer<Odometry, JointState>>(
-        subscriber_odom_, subscriber_joint_state_, rmw_qos_profile.depth);
-  }
-  {
-    using namespace std::placeholders;
-    rclcpp::QoS qos = rclcpp::QoS(10);
-    qos.best_effort();
-    sensor_publisher_ = get_node()->create_publisher<SensorMsg>("sensor", qos);
-    control_subscriber_ = get_node()->create_subscription<ControlMsg>(
-        "control", qos,
-        std::bind(&LinearFeedbackControllerRos::control_subscription_callback,
-                  this, _1));
-  }
+  rclcpp::QoS qos = rclcpp::QoS(10);
+  qos.best_effort();
+  using namespace std::placeholders;
 
+  auto rmw_qos_profile = qos.get_rmw_qos_profile();
+  if (lfc_.get_robot_model()->get_robot_has_free_flyer()) {
+    subscriber_odom_.subscribe(get_node(), "odom", rmw_qos_profile);
+  }
+  subscriber_joint_state_.subscribe(get_node(), "joint_state", rmw_qos_profile);
+  state_syncher_ =
+      std::make_shared<message_filters::TimeSynchronizer<Odometry, JointState>>(
+          subscriber_odom_, subscriber_joint_state_, rmw_qos_profile.depth);
+
+  sensor_publisher_ = get_node()->create_publisher<SensorMsg>("sensor", qos);
+  control_subscriber_ = get_node()->create_subscription<ControlMsg>(
+      "control", qos,
+      std::bind(&LinearFeedbackControllerRos::control_subscription_callback,
+                this, _1));
   return true;
 }
 

src/robot_model_builder.cpp

@@ -1,5 +1,3 @@
-// #include <algorithm>
-// #include <pinocchio/algorithm/compute-all-terms.hpp>
 #include "linear_feedback_controller/robot_model_builder.hpp"
 
 #include <pinocchio/algorithm/frames.hpp>