Admittance robot description param

admittance_controller/include/admittance_controller/admittance_controller.hpp

@@ -133,17 +133,22 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
   // ROS subscribers
   rclcpp::Subscription<trajectory_msgs::msg::JointTrajectoryPoint>::SharedPtr
     input_joint_command_subscriber_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
+    input_goal_pose_subscriber_;
   rclcpp::Publisher<control_msgs::msg::AdmittanceControllerState>::SharedPtr s_publisher_;
 
   // admittance parameters
   std::shared_ptr<admittance_controller::ParamListener> parameter_handler_;
 
   // ROS messages
   std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> joint_command_msg_;
+  std::shared_ptr<geometry_msgs::msg::PoseStamped> goal_pose_msg_;
 
   // real-time buffer
   realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint>>
     input_joint_command_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::PoseStamped>>
+    input_goal_pose_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ControllerStateMsg>> state_publisher_;
 
   trajectory_msgs::msg::JointTrajectoryPoint last_commanded_;

admittance_controller/include/admittance_controller/admittance_rule.hpp

@@ -29,6 +29,8 @@
 #include "kinematics_interface/kinematics_interface.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace admittance_controller
 {
@@ -102,20 +104,38 @@ class AdmittanceRule
 
   /// Configure admittance rule memory using number of joints.
   controller_interface::return_type configure(
-    const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joint);
+    const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joint,
+    const std::string & robot_description);
 
   /// 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
+   * Calculate 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
+   * return value is true if all transformation are calculated without an error 
+   * \param[in] current_joint_state current joint state of the robot 
+   * \param[out] success true if no calls to the kinematics interface fail
+   */  
+  bool get_current_state_transforms(
+    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state);
+  /**
+   * Calculate ft_sensor_link -> base_link transform 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] reference_pose input ft sensor reference pose
+   * \param[out] success true if no calls to the kinematics interface fail
    */
-  bool get_all_transforms(
-    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
+  bool get_reference_state_transforms(
+    const geometry_msgs::msg::PoseStamped & reference_pose);
+  /**
+   * Calculate ft_sensor_link -> base_link transform 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] reference_joint_state input reference joint state
+   * \param[out] success true if no calls to the kinematics interface fail
+   */
+  bool get_reference_state_transforms(
     const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state);
 
   /**
@@ -125,6 +145,24 @@ class AdmittanceRule
    */
   void apply_parameters_update();
 
+  /**
+   * Calculate 'desired joint states' based on the 'measured force', 'reference goal pose', and
+   * 'current_joint_state'.
+   *
+   * \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_pose input reference pose
+   * \param[in] period time in seconds since last controller update
+   * \param[out] desired_joint_state joint state reference after the admittance offset is applied to
+   * the input reference
+   */
+  controller_interface::return_type update(
+    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
+    const geometry_msgs::msg::Wrench & measured_wrench,
+    const geometry_msgs::msg::PoseStamped & reference_pose,
+    const rclcpp::Duration & period,
+    trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states);
+
   /**
    * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and
    * 'current_joint_state'.
@@ -151,12 +189,29 @@ class AdmittanceRule
    */
   const control_msgs::msg::AdmittanceControllerState & get_controller_state();
 
+  /**
+   * Explanation - uses kinematics
+   *
+   * \param[in] state_message message
+   * \param[out] state_message message
+   */
+  geometry_msgs::msg::Pose initialize_goal_pose(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state);
+
 public:
   // admittance config parameters
   std::shared_ptr<admittance_controller::ParamListener> parameter_handler_;
   admittance_controller::Params parameters_;
 
 protected:
+  /**
+   * Populate the AdmittanceState member using the current AdmittanceTransforms
+   * \param[in] current_joint_state
+   * \param[in] measured_wrench
+   */
+  void get_admittance_state_from_transforms(
+    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
+    const geometry_msgs::msg::Wrench & measured_wrench);
+
   /**
    * Calculates the admittance rule from given the robot's current joint angles. The admittance
    * controller state input is updated with the new calculated values. A boolean value is returned

admittance_controller/include/admittance_controller/admittance_rule_impl.hpp

@@ -20,6 +20,7 @@
 #include "admittance_controller/admittance_rule.hpp"
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include <control_toolbox/filters.hpp>
@@ -34,7 +35,8 @@ constexpr auto NUM_CARTESIAN_DOF = 6;  // (3 translation + 3 rotation)
 
 /// Configure admittance rule memory for num joints and load kinematics interface
 controller_interface::return_type AdmittanceRule::configure(
-  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joints)
+  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joints,
+  const std::string & robot_description)
 {
   num_joints_ = num_joints;
 
@@ -52,7 +54,9 @@ controller_interface::return_type AdmittanceRule::configure(
       kinematics_ = std::unique_ptr<kinematics_interface::KinematicsInterface>(
         kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name));
       if (!kinematics_->initialize(
-            node->get_node_parameters_interface(), parameters_.kinematics.tip))
+            robot_description,
+            node->get_node_parameters_interface(),
+            "kinematics"))
       {
         return controller_interface::return_type::ERROR;
       }
@@ -113,6 +117,15 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints)
   return controller_interface::return_type::OK;
 }
 
+geometry_msgs::msg::Pose AdmittanceRule::initialize_goal_pose(
+  const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state)
+{
+  kinematics_->calculate_link_transform(
+    current_joint_state.positions, parameters_.ft_sensor.frame.id,
+    admittance_transforms_.ref_base_ft_);
+  return tf2::toMsg(admittance_transforms_.ref_base_ft_);
+}
+
 void AdmittanceRule::apply_parameters_update()
 {
   if (parameter_handler_->is_old(parameters_))
@@ -134,17 +147,12 @@ 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_);
 
+bool AdmittanceRule::get_current_state_transforms(
+  const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state)
+{
   // get transforms at current configuration
-  success &= kinematics_->calculate_link_transform(
+  bool 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_);
@@ -160,6 +168,20 @@ bool AdmittanceRule::get_all_transforms(
 
   return success;
 }
+bool AdmittanceRule::get_reference_state_transforms(
+  const geometry_msgs::msg::PoseStamped & reference_pose)
+{
+  tf2::fromMsg(reference_pose.pose, admittance_transforms_.ref_base_ft_);
+  return true;
+}
+bool AdmittanceRule::get_reference_state_transforms(
+  const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state)
+{
+  bool success = kinematics_->calculate_link_transform(
+    reference_joint_state.positions, parameters_.ft_sensor.frame.id,
+    admittance_transforms_.ref_base_ft_);
+  return success;
+}
 
 // Update from reference joint states
 controller_interface::return_type AdmittanceRule::update(
@@ -174,27 +196,9 @@ controller_interface::return_type AdmittanceRule::update(
   {
     apply_parameters_update();
   }
-
-  bool success = get_all_transforms(current_joint_state, reference_joint_state);
-
-  // 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);
-
-  // transform wrench_world_ into base frame
-  admittance_state_.wrench_base.block<3, 1>(0, 0) =
-    admittance_transforms_.world_base_.rotation().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);
-
-  // Compute admittance control law
-  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;
+  bool success = get_current_state_transforms(current_joint_state);
+  success &= get_reference_state_transforms(reference_joint_state);
+  get_admittance_state_from_transforms(current_joint_state, measured_wrench);
   success &= calculate_admittance_rule(admittance_state_, dt);
 
   // if a failure occurred during any kinematics interface calls, return an error and don't
@@ -215,10 +219,69 @@ controller_interface::return_type AdmittanceRule::update(
     desired_joint_state.accelerations[i] =
       reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i];
   }
+  return controller_interface::return_type::OK;
+}
+
+controller_interface::return_type AdmittanceRule::update(
+  const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
+  const geometry_msgs::msg::Wrench & measured_wrench,
+  const geometry_msgs::msg::PoseStamped & reference_pose,
+  const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state)
+{
+  const double dt = period.seconds();
+
+  if (parameters_.enable_parameter_update_without_reactivation)
+  {
+    apply_parameters_update();
+  }
+  bool success = get_current_state_transforms(current_joint_state);
+  success &= get_reference_state_transforms(reference_pose);
+  get_admittance_state_from_transforms(current_joint_state, measured_wrench);
+  success &= calculate_admittance_rule(admittance_state_, dt);
+
+  // if a failure occurred during any kinematics interface calls, return an error and
+  // remain at current state
+  if (!success)
+  {
+    desired_joint_state = current_joint_state;
+    return controller_interface::return_type::ERROR;
+  }
+
+  // update joint desired joint state
+  for (size_t i = 0; i < num_joints_; ++i)
+  {
+    desired_joint_state.positions[i] = admittance_state_.joint_pos[i];
+    desired_joint_state.velocities[i] = admittance_state_.joint_vel[i];
+    desired_joint_state.accelerations[i] = admittance_state_.joint_acc[i];
+  }
 
   return controller_interface::return_type::OK;
 }
 
+void AdmittanceRule::get_admittance_state_from_transforms(
+    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
+    const geometry_msgs::msg::Wrench & measured_wrench)
+{
+  // 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);
+
+  // transform wrench_world_ into base frame
+  admittance_state_.wrench_base.block<3, 1>(0, 0) =
+    admittance_transforms_.world_base_.rotation().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);
+
+  // Compute admittance control law
+  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;
+}
+
 bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_state, double dt)
 {
   // Create stiffness matrix in base frame. The user-provided values of admittance_state.stiffness