squash previous:

  - fix CMakeLists
  - add inertia to VIC state msg
  - add "has_valid_wrench" flag to VIC state msg
  - various fixes
  - add verbose

Author: Thibault Poignonec

cartesian_control_msgs/msg/VicControllerState.msg

@@ -5,7 +5,8 @@ std_msgs/String control_mode # admittance / impedance
 # Cartesian compliance reference
 geometry_msgs/Pose desired_pose
 geometry_msgs/Twist desired_velocity
-geometry_msgs/Accel desired_acceleration # optional
+geometry_msgs/Accel desired_acceleration # optional, set to null by default
+geometry_msgs/Wrench desired_wrench # optional, set to null by default
 
 std_msgs/Float64MultiArray desired_inertia
 std_msgs/Float64MultiArray desired_damping
@@ -15,6 +16,7 @@ std_msgs/Float64MultiArray desired_stiffness
 geometry_msgs/Pose pose
 geometry_msgs/Twist velocity
 geometry_msgs/Accel acceleration # optional
+bool has_valid_wrench
 geometry_msgs/Wrench wrench
 
 std_msgs/Float64MultiArray rendered_inertia  # optional

cartesian_vic_controller/CMakeLists.txt

@@ -35,6 +35,8 @@ foreach(Dependency IN ITEMS ${DEPENDENCIES})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
+# Main VIC controller library
+# ---------------------------------
 generate_parameter_library(cartesian_vic_controller_parameters
   src/cartesian_vic_controller_parameters.yaml
 )
@@ -46,11 +48,6 @@ add_library(cartesian_vic_controller SHARED
   src/cartesian_vic_state.cpp
   src/compliance_frame_trajectory.cpp
   src/utils.cpp
-  # Vanilla VIC rules (basic admittance / impedance control laws)
-  src/rules/vanilla_cartesian_admittance_rule.cpp
-  src/rules/vanilla_cartesian_impedance_rule.cpp
-  # Impedance control for Force Dimension Haptic Interfaces
-  src/rules/fd_impedance_rule.cpp
 )
 target_compile_features(cartesian_vic_controller PUBLIC cxx_std_17)
 target_include_directories(cartesian_vic_controller PUBLIC
@@ -67,6 +64,24 @@ ament_target_dependencies(cartesian_vic_controller PUBLIC ${DEPENDENCIES})
 target_compile_definitions(cartesian_vic_controller PRIVATE "CARTESIAN_VIC_CONTROLLER_BUILDING_DLL")
 
 pluginlib_export_plugin_description_file(controller_interface cartesian_vic_controller.xml)
+
+# VIC rules library
+add_library(cartesian_vic_rules SHARED
+  # Vanilla VIC rules (basic admittance / impedance control laws)
+  src/rules/vanilla_cartesian_admittance_rule.cpp
+  src/rules/vanilla_cartesian_impedance_rule.cpp
+  # Impedance control for Force Dimension Haptic Interfaces
+  src/rules/fd_impedance_rule.cpp
+)
+target_compile_features(cartesian_vic_rules PUBLIC cxx_std_17)
+target_include_directories(cartesian_vic_rules PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/cartesian_vic_controller>
+)
+target_link_libraries(cartesian_vic_rules PUBLIC
+  cartesian_vic_controller
+)
+ament_target_dependencies(cartesian_vic_rules PUBLIC ${DEPENDENCIES})
 pluginlib_export_plugin_description_file(cartesian_vic_controller cartesian_vic_rules.xml)
 
 if(BUILD_TESTING)
@@ -95,7 +110,10 @@ if(BUILD_TESTING)
     test/test_cartesian_vic_controller.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml
   )
-  target_link_libraries(test_controller cartesian_vic_controller)
+  target_link_libraries(test_controller
+    cartesian_vic_controller
+    cartesian_vic_rules
+  )
   ament_target_dependencies(test_controller
     control_msgs
     controller_interface
@@ -109,7 +127,11 @@ install(
   DESTINATION include/cartesian_vic_controller
 )
 
-install(TARGETS cartesian_vic_controller cartesian_vic_controller_parameters
+install(
+  TARGETS
+    cartesian_vic_controller
+    cartesian_vic_controller_parameters
+    cartesian_vic_rules
   EXPORT export_cartesian_vic_controller
   RUNTIME DESTINATION bin
   ARCHIVE DESTINATION lib

cartesian_vic_controller/cartesian_vic_rules.xml

@@ -1,4 +1,4 @@
-<library path="cartesian_vic_controller">
+<library path="cartesian_vic_rules">
   <class name="cartesian_vic_controller/VanillaCartesianAdmittanceRule"
          type="cartesian_vic_controller::VanillaCartesianAdmittanceRule"
          base_class_type="cartesian_vic_controller::CartesianVicRule">

cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_rule.hpp

@@ -203,6 +203,9 @@ class CartesianVicRule
   /// Filtered wrench expressed in robot base frame
   Eigen::Matrix<double, 6, 1> wrench_base_;
 
+  /// Jacobian pre-allocation (used to compute cartesian inertia matrix)
+  Eigen::Matrix<double, 6, Eigen::Dynamic> J_private_;
+
   /// Filtered external torques
   Eigen::VectorXd filtered_external_torques_;
 };

cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp

@@ -52,17 +52,22 @@ class VicInputData
     joint_state_velocity = Eigen::VectorXd::Zero(num_joints);
     joint_state_external_torques_ = Eigen::VectorXd::Zero(num_joints);
 
+    natural_joint_space_inertia = \
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(num_joints, num_joints);
+
     // Allocate nullspace parameters
     nullspace_desired_joint_positions = Eigen::VectorXd::Zero(num_joints);
     nullspace_joint_inertia = Eigen::VectorXd::Zero(num_joints);
     nullspace_joint_stiffness = Eigen::VectorXd::Zero(num_joints);
     nullspace_joint_damping = Eigen::VectorXd::Zero(num_joints);
 
-    // Allocate cartesian state
-    /*
+    // Reset cartesian state
+    robot_current_velocity.setZero();
+    robot_estimated_acceleration.setZero();
+    robot_current_wrench_at_ft_frame_.setZero();
     natural_joint_space_inertia = \
       Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(num_joints, num_joints);
-    */
+
     // Reset data availability
     has_ft_sensor_ = false;
     has_external_torque_sensor_ = false;
@@ -150,10 +155,11 @@ class VicInputData
   // Cartesian state (control frame w.r.t. robot base frame)
   Eigen::Isometry3d robot_current_pose;
   Eigen::Matrix<double, 6, 1> robot_current_velocity;
+  Eigen::Matrix<double, 6, 1> robot_estimated_acceleration;
 
   /// Natural inertia matrix (from dynamic model)
-  // Eigen::Matrix<double, 6, 6> natural_cartesian_inertia;
-  // Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> natural_joint_space_inertia;
+  Eigen::Matrix<double, 6, 6> natural_cartesian_inertia;
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> natural_joint_space_inertia;
 
 protected:
   Eigen::VectorXd joint_state_external_torques_;

cartesian_vic_controller/include/cartesian_vic_controller/vic_msgs_utils.hpp

@@ -0,0 +1,168 @@
+// Copyright 2024, ICube Laboratory, University of Strasbourg
+//
+// 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.
+
+
+//-----------------------------------------------------------------------------
+/*!\file    vic_msg_utils.hpp
+ *
+ * \author  thibault Poignonec <tpoignonec@unistra.fr>
+ * \date    2024/08/07
+ */
+//-----------------------------------------------------------------------------
+
+#ifndef CARTESIAN_VIC_CONTROLLER__VIC_MSGS_UTILS_HPP_
+#define CARTESIAN_VIC_CONTROLLER__VIC_MSGS_UTILS_HPP_
+
+
+// Misc.
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include <string>
+#include <memory>
+
+#include "tf2_eigen/tf2_eigen.hpp"
+#include "tf2_kdl/tf2_kdl.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+// Custom msgs
+#include "cartesian_control_msgs/msg/vic_controller_state.hpp"
+#include "cartesian_control_msgs/msg/cartesian_trajectory.hpp"
+#include "cartesian_control_msgs/msg/compliant_frame_trajectory.hpp"
+#include "cartesian_vic_controller/utils.hpp"
+
+
+namespace cartesian_vic_controller
+{
+
+// Read VIC state message
+
+bool get_robot_pose(
+  const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
+  Eigen::Affine3d & pose)
+{
+  tf2::fromMsg(vic_state_msg.pose, pose);
+  return true;
+}
+
+bool get_robot_velocity(
+  const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
+  Eigen::Matrix<double, 6, 1> & twist)
+{
+  tf2::fromMsg(vic_state_msg.velocity, twist);
+  return true;
+}
+
+bool get_robot_acceleration(
+  const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
+  Eigen::Matrix<double, 6, 1> & acceleration)
+{
+  acceleration[0] = vic_state_msg.acceleration.linear.x;
+  acceleration[1] = vic_state_msg.acceleration.linear.y;
+  acceleration[2] = vic_state_msg.acceleration.linear.z;
+  acceleration[3] = vic_state_msg.acceleration.angular.x;
+  acceleration[4] = vic_state_msg.acceleration.angular.y;
+  acceleration[5] = vic_state_msg.acceleration.angular.z;
+  return true;
+}
+
+bool get_robot_wrench(
+  const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
+  Eigen::Matrix<double, 6, 1> & wrench)
+{
+  wrench[0] = vic_state_msg.wrench.force.x;
+  wrench[1] = vic_state_msg.wrench.force.y;
+  wrench[2] = vic_state_msg.wrench.force.z;
+  wrench[3] = vic_state_msg.wrench.torque.x;
+  wrench[4] = vic_state_msg.wrench.torque.y;
+  wrench[5] = vic_state_msg.wrench.torque.z;
+  return true;
+}
+
+bool get_desired_robot_pose(
+  const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
+  Eigen::Affine3d & desired_pose)
+{
+  tf2::fromMsg(vic_state_msg.desired_pose, desired_pose);
+  return true;
+}
+bool get_desired_robot_velocity(
+  const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
+  Eigen::Matrix<double, 6, 1> & desired_twist)
+{
+  tf2::fromMsg(vic_state_msg.desired_velocity, desired_twist);
+  return true;
+}
+bool get_desired_robot_acceleration(
+  const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
+  Eigen::Matrix<double, 6, 1> & desired_acceleration)
+{
+  desired_acceleration[0] = vic_state_msg.desired_acceleration.linear.x;
+  desired_acceleration[1] = vic_state_msg.desired_acceleration.linear.y;
+  desired_acceleration[2] = vic_state_msg.desired_acceleration.linear.z;
+  desired_acceleration[3] = vic_state_msg.desired_acceleration.angular.x;
+  desired_acceleration[4] = vic_state_msg.desired_acceleration.angular.y;
+  desired_acceleration[5] = vic_state_msg.desired_acceleration.angular.z;
+  return true;
+}
+bool get_desired_robot_wrench(
+  const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
+  Eigen::Matrix<double, 6, 1> & desired_wrench)
+{
+  desired_wrench[0] = vic_state_msg.desired_wrench.force.x;
+  desired_wrench[1] = vic_state_msg.desired_wrench.force.y;
+  desired_wrench[2] = vic_state_msg.desired_wrench.force.z;
+  desired_wrench[3] = vic_state_msg.desired_wrench.torque.x;
+  desired_wrench[4] = vic_state_msg.desired_wrench.torque.y;
+  desired_wrench[5] = vic_state_msg.desired_wrench.torque.z;
+  return true;
+}
+
+bool get_current_impedance_profile(
+  const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
+  Eigen::Matrix<double, 6, 6> & M,
+  Eigen::Matrix<double, 6, 6> & K,
+  Eigen::Matrix<double, 6, 6> & D)
+{
+  bool success = fromMsg(vic_state_msg.rendered_inertia, M);
+  success &= fromMsg(vic_state_msg.rendered_stiffness, K);
+  success &= fromMsg(vic_state_msg.rendered_damping, D);
+  return success;
+}
+
+bool get_desired_impedance_profile(
+  const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
+  Eigen::Matrix<double, 6, 6> & M_d,
+  Eigen::Matrix<double, 6, 6> & K_d,
+  Eigen::Matrix<double, 6, 6> & D_d)
+{
+  bool success = fromMsg(vic_state_msg.desired_inertia, M_d);
+  success &= fromMsg(vic_state_msg.desired_stiffness, K_d);
+  success &= fromMsg(vic_state_msg.desired_damping, D_d);
+  return success;
+}
+
+bool get_natural_robot_inertia(
+  const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
+  Eigen::Matrix<double, 6, 6> & M_natural)
+{
+  bool success = fromMsg(vic_state_msg.natural_inertia, M_natural);
+  return success;
+}
+
+
+}  // namespace cartesian_vic_controller
+
+#endif  // CARTESIAN_VIC_CONTROLLER__VIC_MSGS_UTILS_HPP_

cartesian_vic_controller/src/cartesian_vic_rule.cpp

@@ -90,6 +90,10 @@ CartesianVicRule::configure(
     return controller_interface::return_type::ERROR;
   }
 
+  // Allocate temporary matrices
+  J_private_ = \
+    Eigen::Matrix<double, 6, Eigen::Dynamic>::Zero(6, num_joints);
+
   return controller_interface::return_type::OK;
 }
 
@@ -437,7 +441,24 @@ CartesianVicRule::update_input_data(
       measurement_data.get_joint_state()
   );
 
+  // Retrieve inertia matrices
+  success &= dynamics_->calculate_inertia(
+    vic_state_.input_data.joint_state_position,
+    vic_state_.input_data.natural_joint_space_inertia
+  );
+  success &= dynamics_->calculate_jacobian(
+    vic_state_.input_data.joint_state_position,
+    vic_state_.input_data.control_frame,
+    J_private_
+  );
+  vic_state_.input_data.natural_cartesian_inertia = (J_private_ * \
+    vic_state_.input_data.natural_joint_space_inertia.inverse() * \
+    J_private_.transpose()).inverse();
+
   if (!success) {
+    RCLCPP_ERROR(
+        logger_,
+        "update_input_data(): failed to update input data!");
     return controller_interface::return_type::ERROR;
   }
   return controller_interface::return_type::OK;

cartesian_vic_controller/src/cartesian_vic_state.cpp

@@ -60,6 +60,7 @@ VicState::to_msg(cartesian_control_msgs::msg::VicControllerState & vic_state_msg
   vic_state_msg.desired_pose = Eigen::toMsg(desired_frame_0.pose);
   vic_state_msg.desired_velocity = Eigen::toMsg(desired_frame_0.velocity);
   vic_state_msg.desired_acceleration = AccelToMsg(desired_frame_0.acceleration);
+  vic_state_msg.desired_wrench = WrenchToMsg(desired_frame_0.wrench);
   matrixEigenToMsg(desired_frame_0.inertia, vic_state_msg.desired_inertia);
   matrixEigenToMsg(desired_frame_0.stiffness, vic_state_msg.desired_stiffness);
   matrixEigenToMsg(desired_frame_0.damping, vic_state_msg.desired_damping);
@@ -68,16 +69,18 @@ VicState::to_msg(cartesian_control_msgs::msg::VicControllerState & vic_state_msg
   vic_state_msg.pose = Eigen::toMsg(input_data.robot_current_pose);
   vic_state_msg.velocity = Eigen::toMsg(input_data.robot_current_velocity);
   if (input_data.has_ft_sensor()) {
+    vic_state_msg.has_valid_wrench = true;
     vic_state_msg.wrench = WrenchToMsg(input_data.get_ft_sensor_wrench());
   } else {
+    vic_state_msg.has_valid_wrench = false;
     vic_state_msg.wrench.force.x = 0.0;
     vic_state_msg.wrench.force.y = 0.0;
     vic_state_msg.wrench.force.z = 0.0;
     vic_state_msg.wrench.torque.x = 0.0;
     vic_state_msg.wrench.torque.y = 0.0;
     vic_state_msg.wrench.torque.z = 0.0;
   }
-  // matrixEigenToMsg(input_data.natural_cartesian_inertia, vic_state_msg.natural_inertia);
+  matrixEigenToMsg(input_data.natural_cartesian_inertia, vic_state_msg.natural_inertia);
 
   // Fill rendered impedance
   matrixEigenToMsg(command_data.inertia, vic_state_msg.rendered_inertia);

cartesian_vic_controller/src/utils.cpp

@@ -45,6 +45,9 @@ geometry_msgs::msg::Wrench WrenchToMsg(const Eigen::Matrix<double, 6, 1> & in)
 
 bool fromMsg(const std_msgs::msg::Float64MultiArray & m, Eigen::Matrix<double, 6, 6> & e)
 {
+  if (m.data.size() != 36) {
+    return false;
+  }
   /*
   // TODO(tpoignonec): check layout validity !!! (could be flattened or not...)
   size_t size_in_msg = m.layout.dim[0].size;