enable pose-only goal in AdmittanceRule

Author: Nibanovic

admittance_controller/include/admittance_controller/admittance_rule.hpp

@@ -29,6 +29,7 @@
 #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"
 
 namespace admittance_controller
 {
@@ -111,13 +112,14 @@ class AdmittanceRule
   /**
    * 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
+   * 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_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,
-    const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state);
+    const geometry_msgs::msg::Pose & reference_pose);
 
   /**
    * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent
@@ -132,14 +134,15 @@ class AdmittanceRule
    *
    * \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
+   * \param[in] reference_pose input joint state reference
    * \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::Pose & reference_pose,
     const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state,
     const rclcpp::Duration & period,
     trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states);
@@ -152,6 +155,14 @@ 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/include/admittance_controller/admittance_rule_impl.hpp

@@ -115,6 +115,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_))
@@ -136,14 +145,14 @@ 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)
+  const geometry_msgs::msg::Pose & reference_pose)
 {
-  // get reference transforms
-  bool success = kinematics_->calculate_link_transform(
-    reference_joint_state.positions, parameters_.ft_sensor.frame.id,
-    admittance_transforms_.ref_base_ft_);
+  // get reference transforms 
+  bool success=true;  
+  tf2::fromMsg(reference_pose, admittance_transforms_.ref_base_ft_);
 
   // get transforms at current configuration
   success &= kinematics_->calculate_link_transform(
@@ -167,7 +176,8 @@ bool AdmittanceRule::get_all_transforms(
 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 geometry_msgs::msg::Pose & reference_pose,
+  const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, 
   const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state)
 {
   const double dt = period.seconds();
@@ -176,9 +186,7 @@ controller_interface::return_type AdmittanceRule::update(
   {
     apply_parameters_update();
   }
-
-  bool success = get_all_transforms(current_joint_state, reference_joint_state);
-
+  bool success = get_all_transforms(current_joint_state, reference_pose);
   // 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();

admittance_controller/src/admittance_controller.cpp

@@ -404,8 +404,17 @@ controller_interface::return_type AdmittanceController::update_and_write_command
   // get all controller inputs
   read_state_from_hardware(joint_state_, ft_values_);
 
+  auto goal_pose = geometry_msgs::msg::Pose();
+  /*   world-tool0*/
+  goal_pose.position.x = 1.53;
+  goal_pose.position.y = 0.9;
+  goal_pose.position.z = 1.5;
+  goal_pose.orientation.w = 0.0;
+  goal_pose.orientation.x = 0.707;
+  goal_pose.orientation.y = 0.0;
+  goal_pose.orientation.z = 0.707;
   // apply admittance control to reference to determine desired state
-  admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_);
+  admittance_->update(joint_state_, ft_values_, goal_pose, reference_, period, reference_admittance_);
 
   // write calculated values to joint interfaces
   write_state_to_hardware(reference_admittance_);