Cartesian goal conversion

admittance_controller/include/admittance_controller/admittance_controller.hpp

@@ -143,6 +143,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
   // ROS messages
   std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> joint_command_msg_;
   std::shared_ptr<geometry_msgs::msg::PoseStamped> goal_pose_msg_;
+  geometry_msgs::msg::PoseStamped last_goal_pose_;
 
   // real-time buffer
   realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint>>
@@ -159,9 +160,17 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
   // joint_state_: current joint readings from the hardware
   // reference_admittance_: reference value used by the controller after the admittance values are
   // applied ft_values_: values read from the force torque sensor
-  trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_;
+  trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_, joint_space_goal_;
   geometry_msgs::msg::Wrench ft_values_;
 
+  /**
+   * @brief Check whether two poses differ within a certain threshold.
+   */
+  bool is_same_pose(
+    const geometry_msgs::msg::Pose & a,
+    const geometry_msgs::msg::Pose & b,
+    double eps = 1e-5);
+
   /**
    * @brief Read values from hardware interfaces and set corresponding fields of state_current and
    * ft_values

admittance_controller/include/admittance_controller/admittance_rule.hpp

@@ -111,31 +111,14 @@ class AdmittanceRule
   controller_interface::return_type reset(const size_t num_joints);
 
   /**
-   * Calculate transforms needed for admittance control using the loader kinematics plugin. If
+   * 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[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_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
+   * 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_reference_state_transforms(
+  bool get_all_transforms(
+    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
     const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state);
 
   /**
@@ -145,24 +128,6 @@ 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'.
@@ -192,10 +157,20 @@ class AdmittanceRule
   /**
    * Explanation - uses kinematics
    *
-   * \param[in] state_message message
-   * \param[out] state_message message
+   * \param[in] current_joint_state message
    */
   geometry_msgs::msg::Pose initialize_goal_pose(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state);
+
+  /**
+   * @brief Convert cartesian goal pose to joint-space goal.
+   * \param[in] input PoseStamped goal expressed as ft_sensor link pose in base_link frame
+   * \param[in] current_state current joint state
+   * \param[out] output = current_state + delta_theta required to reach the goal pose
+   */
+  void cartesian_goal_to_joint_space(
+    const geometry_msgs::msg::PoseStamped & input,
+    trajectory_msgs::msg::JointTrajectoryPoint & current_state,
+    trajectory_msgs::msg::JointTrajectoryPoint & output);
 
 public:
   // admittance config parameters

admittance_controller/include/admittance_controller/admittance_rule_impl.hpp

@@ -126,6 +126,58 @@ geometry_msgs::msg::Pose AdmittanceRule::initialize_goal_pose(
   return tf2::toMsg(admittance_transforms_.ref_base_ft_);
 }
 
+void AdmittanceRule::cartesian_goal_to_joint_space(
+    const geometry_msgs::msg::PoseStamped & input,
+    trajectory_msgs::msg::JointTrajectoryPoint & current_state,
+    trajectory_msgs::msg::JointTrajectoryPoint & output)
+{
+  // calculate difference between `tool0` current and goal pose, 
+  // both expressed in `base_link` frame.
+  Eigen::Isometry3d goal_base_ft;
+  tf2::fromMsg(input.pose, goal_base_ft);
+
+  Eigen::Matrix<double, 6, 1> delta_X;
+  delta_X.block<3, 1>(0, 0) =
+    goal_base_ft.translation() - admittance_transforms_.base_ft_.translation();
+
+  const auto R_current = admittance_transforms_.base_ft_.rotation();
+  const auto R_desired = goal_base_ft.rotation();
+  const auto R = R_desired * R_current.transpose();
+  const auto angle_axis = Eigen::AngleAxisd(R);
+  delta_X.block<3, 1>(3, 0) = angle_axis.angle() * angle_axis.axis();
+
+
+  std::cout<<input.header.frame_id<<" delta_X translation:"<<std::endl;
+  std::cout<<delta_X(0,0)<<std::endl;
+  std::cout<<delta_X(1,0)<<std::endl;
+  std::cout<<delta_X(2,0)<<std::endl;
+
+  std::cout<<input.header.frame_id<<" delta_X rotation:"<<std::endl;
+  std::cout<<delta_X(3,0)<<std::endl;
+  std::cout<<delta_X(4,0)<<std::endl;
+  std::cout<<delta_X(5,0)<<std::endl;
+  std::cout<<"-------------"<<std::endl;
+
+  Eigen::VectorXd delta_joints = Eigen::VectorXd::Zero(6);
+  bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas(
+    admittance_state_.current_joint_pos, delta_X, parameters_.control.frame.id, delta_joints);
+
+  /**/
+  if(success){
+    std::cout<<"VectorXd: "<<std::endl;
+    for(size_t i = 0; i < 6; i++)
+    {
+      std::cout<<delta_joints(i)<<std::endl;
+    }
+  }else{
+    std::cout<<"couldn't convert."<<std::endl;
+  }
+
+
+  for(size_t i = 0; i < output.positions.size(); i++)
+    output.positions[i] = current_state.positions[i] + delta_joints(i);
+}
+
 void AdmittanceRule::apply_parameters_update()
 {
   if (parameter_handler_->is_old(parameters_))
@@ -148,11 +200,17 @@ void AdmittanceRule::apply_parameters_update()
 }
 
 
-bool AdmittanceRule::get_current_state_transforms(
-  const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state)
+bool AdmittanceRule::get_all_transforms(
+  const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
+  const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state)
 {
-  // get transforms at current configuration
+  // 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_);
@@ -168,20 +226,6 @@ bool AdmittanceRule::get_current_state_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(
@@ -196,8 +240,7 @@ controller_interface::return_type AdmittanceRule::update(
   {
     apply_parameters_update();
   }
-  bool success = get_current_state_transforms(current_joint_state);
-  success &= get_reference_state_transforms(reference_joint_state);
+  bool success = get_all_transforms(current_joint_state, reference_joint_state);
   get_admittance_state_from_transforms(current_joint_state, measured_wrench);
   success &= calculate_admittance_rule(admittance_state_, dt);
 
@@ -222,42 +265,6 @@ controller_interface::return_type AdmittanceRule::update(
   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)

admittance_controller/src/admittance_controller.cpp

@@ -360,10 +360,12 @@ controller_interface::CallbackReturn AdmittanceController::on_activate(
     RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize goal_pose from current joint positions.\n");
     return controller_interface::CallbackReturn::ERROR;
   }
+  last_goal_pose_.pose = goal_pose_msg_->pose;
 
   // Use current joint_state as a default reference
   last_reference_ = joint_state_;
   last_commanded_ = joint_state_;
+  joint_space_goal_ = joint_state_;
   reference_ = joint_state_;
   reference_admittance_ = joint_state_;
 
@@ -394,11 +396,25 @@ controller_interface::return_type AdmittanceController::update_reference_from_su
     }
   }
 
-  // after initializing goal_pose_msg_, update it from subscribers only
-  // if another message exists
+  // if another message exists...
   if(*input_goal_pose_.readFromRT())
   {
     goal_pose_msg_ = *input_goal_pose_.readFromRT();
+    // ... and its a different goal...
+    if(is_same_pose(last_goal_pose_.pose, goal_pose_msg_->pose) == false)
+    {
+      // ... convert goal to joint-space and apply it.
+      admittance_->cartesian_goal_to_joint_space(*goal_pose_msg_, joint_state_, joint_space_goal_);
+      for (size_t i = 0; i < joint_space_goal_.positions.size(); ++i)
+      {
+        position_reference_[i].get() = joint_space_goal_.positions[i];
+      }
+      for (size_t i = 0; i < joint_space_goal_.velocities.size(); ++i)
+      {
+        velocity_reference_[i].get() = joint_space_goal_.velocities[i];
+      }
+    }
+    last_goal_pose_.pose = goal_pose_msg_->pose;
   }
 
 
@@ -427,18 +443,7 @@ controller_interface::return_type AdmittanceController::update_and_write_command
   read_state_from_hardware(joint_state_, ft_values_);
 
   // apply admittance control to reference to determine desired state
-
-  /* some flag that determines whether we call the update() with goal_pose or with joint reference.
-    How can we know here whether the reference was updated from subscribers or from itnerfaces?
-
-  if(updated_from_subscribers)
-    admittance_->update(joint_state_, ft_values_, *goal_pose_msg_, period, reference_admittance_);
-  else
-    admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_);
-
-  */
-
-  admittance_->update(joint_state_, ft_values_, *goal_pose_msg_, period, reference_admittance_);
+  admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_);
 
   // write calculated values to joint interfaces
   write_state_to_hardware(reference_admittance_);
@@ -610,6 +615,25 @@ void AdmittanceController::read_state_reference_interfaces(
   last_reference_.velocities = state_reference.velocities;
 }
 
+bool AdmittanceController::is_same_pose(const geometry_msgs::msg::Pose & a,
+                                        const geometry_msgs::msg::Pose & b,
+                                        double eps)
+{
+  // compare position
+  bool pos = std::abs(a.position.x - b.position.x) <= eps &&
+             std::abs(a.position.y - b.position.y) <= eps &&
+             std::abs(a.position.z - b.position.z) <= eps;
+  if (!pos) return false;
+
+  // compare quaternion
+  bool rot = std::abs(a.orientation.w - b.orientation.w) <= eps &&
+             std::abs(a.orientation.x - b.orientation.x) <= eps &&
+             std::abs(a.orientation.y - b.orientation.y) <= eps &&
+             std::abs(a.orientation.z - b.orientation.z) <= eps;
+
+  return rot;
+}
+
 }  // namespace admittance_controller
 
 #include "pluginlib/class_list_macros.hpp"