[AdmittanceController] Remove superfluous admittance_tranforms_ structure (ros-controls#1668)

Author: mgeethik

admittance_controller/include/admittance_controller/admittance_rule.hpp

@@ -32,26 +32,6 @@
 
 namespace admittance_controller
 {
-struct AdmittanceTransforms
-{
-  // transformation from force torque sensor frame to base link frame at reference joint angles
-  Eigen::Isometry3d ref_base_ft_;
-  // transformation from force torque sensor frame to base link frame at reference + admittance
-  // offset joint angles
-  Eigen::Isometry3d base_ft_;
-  // transformation from control frame to base link frame at reference + admittance offset joint
-  // angles
-  Eigen::Isometry3d base_control_;
-  // transformation from end effector frame to base link frame at reference + admittance offset
-  // joint angles
-  Eigen::Isometry3d base_tip_;
-  // transformation from center of gravity frame to base link frame at reference + admittance offset
-  // joint angles
-  Eigen::Isometry3d base_cog_;
-  // transformation from world frame to base link frame
-  Eigen::Isometry3d world_base_;
-};
-
 struct AdmittanceState
 {
   explicit AdmittanceState(size_t num_joints)
@@ -109,17 +89,6 @@ class AdmittanceRule
   /// Reset all values back to default
   controller_interface::return_type reset(const size_t num_joints);
 
-  /**
-   * Calculate all transforms needed for admittance control using the loader kinematics plugin. If
-   * the transform does not exist in the kinematics model, then TF will be used for lookup. The
-   * return value is true if all transformation are calculated without an error \param[in]
-   * current_joint_state current joint state of the robot \param[in] reference_joint_state input
-   * joint state reference \param[out] success true if no calls to the kinematics interface fail
-   */
-  bool get_all_transforms(
-    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
-    const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state);
-
   /**
    * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent
    * Eigen field members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes,
@@ -131,6 +100,10 @@ class AdmittanceRule
    * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and
    * 'current_joint_state'.
    *
+   * All transforms (e.g., world to base, sensor to base, CoG to base) are now computed
+   * directly in this function and stored in `admittance_state_`, removing the
+   * need for an intermediate transform struct.
+   *
    * \param[in] current_joint_state current joint state of the robot
    * \param[in] measured_wrench most recent measured wrench from force torque sensor
    * \param[in] reference_joint_state input joint state reference
@@ -198,9 +171,6 @@ class AdmittanceRule
   // admittance controllers internal state
   AdmittanceState admittance_state_{0};
 
-  // transforms needed for admittance update
-  AdmittanceTransforms admittance_transforms_;
-
   // position of center of gravity in cog_frame
   Eigen::Vector3d cog_pos_;
 

admittance_controller/include/admittance_controller/admittance_rule_impl.hpp

@@ -108,9 +108,6 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints)
   // reset admittance state
   admittance_state_ = AdmittanceState(num_joints);
 
-  // reset transforms and rotations
-  admittance_transforms_ = AdmittanceTransforms();
-
   // reset forces
   wrench_world_.setZero();
   end_effector_weight_.setZero();
@@ -144,78 +141,75 @@ void AdmittanceRule::apply_parameters_update()
   }
 }
 
-bool AdmittanceRule::get_all_transforms(
-  const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
-  const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state)
-{
-  // get reference transforms
-  bool success = kinematics_->calculate_link_transform(
-    reference_joint_state.positions, parameters_.ft_sensor.frame.id,
-    admittance_transforms_.ref_base_ft_);
-
-  // get transforms at current configuration
-  success &= kinematics_->calculate_link_transform(
-    current_joint_state.positions, parameters_.ft_sensor.frame.id, admittance_transforms_.base_ft_);
-  success &= kinematics_->calculate_link_transform(
-    current_joint_state.positions, parameters_.kinematics.tip, admittance_transforms_.base_tip_);
-  success &= kinematics_->calculate_link_transform(
-    current_joint_state.positions, parameters_.fixed_world_frame.frame.id,
-    admittance_transforms_.world_base_);
-  success &= kinematics_->calculate_link_transform(
-    current_joint_state.positions, parameters_.gravity_compensation.frame.id,
-    admittance_transforms_.base_cog_);
-  success &= kinematics_->calculate_link_transform(
-    current_joint_state.positions, parameters_.control.frame.id,
-    admittance_transforms_.base_control_);
-
-  return success;
-}
-
 // Update from reference joint states
 controller_interface::return_type AdmittanceRule::update(
   const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
   const geometry_msgs::msg::Wrench & measured_wrench,
   const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state,
   const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state)
 {
+  // duration & live-parameter refresh
   const double dt = period.seconds();
 
   if (parameters_.enable_parameter_update_without_reactivation)
   {
     apply_parameters_update();
   }
 
-  bool success = get_all_transforms(current_joint_state, reference_joint_state);
+  bool success = true;
 
-  // apply filter and update wrench_world_ vector
-  Eigen::Matrix<double, 3, 3> rot_world_sensor =
-    admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation();
-  Eigen::Matrix<double, 3, 3> rot_world_cog =
-    admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation();
-  process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog);
+  // Reusable transform variable
+  Eigen::Isometry3d tf;
 
-  // transform wrench_world_ into base frame
+  // --- FT sensor frame to base frame (translation + rotation) ---
+  success &= kinematics_->calculate_link_transform(
+    reference_joint_state.positions, parameters_.ft_sensor.frame.id, tf);
+  admittance_state_.ref_trans_base_ft = tf;
+
+  // --- world frame to base frame (we only need the rotation) ---
+  success &= kinematics_->calculate_link_transform(
+    current_joint_state.positions, parameters_.fixed_world_frame.frame.id, tf);
+  const Eigen::Matrix3d rot_world_base = tf.rotation();
+
+  // --- control/base frame to base frame (rotation only) ---
+  success &= kinematics_->calculate_link_transform(
+    current_joint_state.positions, parameters_.control.frame.id, tf);
+  admittance_state_.rot_base_control = tf.rotation();
+
+  success &= kinematics_->calculate_link_transform(
+    current_joint_state.positions, parameters_.gravity_compensation.frame.id, tf);
+  const Eigen::Matrix3d rot_tf_cog = tf.rotation();
+
+  success &= kinematics_->calculate_link_transform(
+    current_joint_state.positions, parameters_.ft_sensor.frame.id, tf);
+  const Eigen::Matrix3d rot_tf_base_ft = tf.rotation();
+
+  // wrench processing (gravity + filter) in world
+  process_wrench_measurements(
+    measured_wrench,
+    // pass rotations into sensor and CoG:
+    rot_world_base * rot_tf_base_ft, rot_world_base * rot_tf_cog);
+
+  // transform filtered wrench into the robot base frame
   admittance_state_.wrench_base.block<3, 1>(0, 0) =
-    admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0);
+    rot_world_base.transpose() * wrench_world_.block<3, 1>(0, 0);
   admittance_state_.wrench_base.block<3, 1>(3, 0) =
-    admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0);
+    rot_world_base.transpose() * wrench_world_.block<3, 1>(3, 0);
 
-  // Compute admittance control law
+  // populate current joint positions in the state
   vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos);
-  admittance_state_.rot_base_control = admittance_transforms_.base_control_.rotation();
-  admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_;
+
   admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id;
-  success &= calculate_admittance_rule(admittance_state_, dt);
 
-  // if a failure occurred during any kinematics interface calls, return an error and don't
-  // modify the desired reference
+  // compute admittance dynamics
+  success &= calculate_admittance_rule(admittance_state_, dt);
   if (!success)
   {
     desired_joint_state = reference_joint_state;
     return controller_interface::return_type::ERROR;
   }
 
-  // update joint desired joint state
+  // Update final desired_joint_state using the computed transforms
   for (size_t i = 0; i < num_joints_; ++i)
   {
     auto idx = static_cast<Eigen::Index>(i);