prepare admittance for singularity checks

Author: Thibault Poignonec

cartesian_vic_controller/src/rules/vanilla_cartesian_admittance_rule.cpp

@@ -71,6 +71,9 @@ bool VanillaCartesianAdmittanceRule::compute_controls(
     success = false;
   }
 
+  size_t num_joints = vic_state_.input_data.joint_state_position.size();
+  size_t dims = 6;  // 6 DoF
+
   // auto num_joints = vic_input_data.joint_state_position.size();
   // Get reference compliant frame at t_k
   RCLCPP_DEBUG(logger_, "Reading reference frame...");
@@ -180,9 +183,6 @@ bool VanillaCartesianAdmittanceRule::compute_controls(
     J_dot_
   );
 
-  RCLCPP_DEBUG(logger_, "Computing J_pinv...");
-  J_pinv_ = (J_.transpose() * J_ + alpha_pinv_ * I_joint_space_).inverse() * J_.transpose();
-
   if (!model_is_ok) {
     success = false;
     RCLCPP_ERROR(
@@ -191,6 +191,21 @@ bool VanillaCartesianAdmittanceRule::compute_controls(
     );
   }
 
+
+  RCLCPP_DEBUG(logger_, "Computing J_pinv...");
+  const Eigen::JacobiSVD<Eigen::MatrixXd> J_svd =
+    Eigen::JacobiSVD<Eigen::MatrixXd>(J_, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  const Eigen::MatrixXd matrix_s = J_svd.singularValues().asDiagonal();
+  if (J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1) > 30) {
+    success = false;
+    RCLCPP_ERROR(
+      logger_,
+      "Jacobian singularity detected!"
+    );
+  }
+  // J_pinv_ = J_svd.matrixV() * matrix_s.inverse() * J_svd.matrixU().transpose();
+  J_pinv_ = (J_.transpose() * J_ + alpha_pinv_ * I_joint_space_).inverse() * J_.transpose();
+
   // Compute desired inertia matrix and its inverse
   RCLCPP_DEBUG(logger_, "Computing M_inv...");
   Eigen::Matrix<double, 6, 6> M_inv;
@@ -214,33 +229,72 @@ bool VanillaCartesianAdmittanceRule::compute_controls(
   // VIC rule: M * err_p_ddot + D * err_p_dot + K * err_p = F_ext - F_ref
   // where err_p = p_desired - p_current
   //
-  // -> commanded_acc = p_ddot_desired + inv(M) * (K * err_p + D * err_p_dot - F_ext + F_ref)
-  // Implement "normal" impedance control
-  RCLCPP_DEBUG(logger_, "Cmd cartesian acc...");
-  Eigen::Matrix<double, 6, 1> commanded_cartesian_acc = reference_compliant_frame.acceleration + \
-    M_inv * (K * error_pose + D * error_velocity - F_ext + reference_compliant_frame.wrench);
-
-  // TODO(tpoignonec): clip cartesian acceleration in min/max range
-  // (and same for velocity if possible)
 
   /*
-  // Basic scheme for reference (not great): integrate cartesian velocity
-
-  // Compute commanded cartesian twist
-  auto robot_command_twist = J_ * previous_jnt_cmd_velocity + commanded_cartesian_acc * dt;
+  auto robot_command_twist = D.inverse() * (
+    K * error_pose
+    + M * (reference_compliant_frame.acceleration - vic_input_data.robot_estimated_acceleration)
+    - F_ext  + reference_compliant_frame.wrench
+    ) + reference_compliant_frame.velocity;
   success &= dynamics_->convert_cartesian_deltas_to_joint_deltas(
     vic_input_data.joint_state_position,
     robot_command_twist,
     vic_input_data.control_frame,
     vic_command_data.joint_command_velocity
   );
+
+  // Nullspace objective for stability
+  // ------------------------------------------------
+  if (false){ //(vic_input_data.activate_nullspace_control) {
+    RCLCPP_DEBUG(logger_, "Cmd nullspace joint acc...");
+    nullspace_projection_ = I_joint_space_ - J_pinv_ * J_;
+    M_nullspace_.diagonal() = vic_input_data.nullspace_joint_inertia;
+    K_nullspace_.diagonal() = vic_input_data.nullspace_joint_stiffness;
+    D_nullspace_.diagonal() = vic_input_data.nullspace_joint_damping;
+    auto D_inv_nullspace_ = D_nullspace_;
+    D_inv_nullspace_.diagonal() = D_nullspace_.diagonal().cwiseInverse();
+    auto error_position_nullspace = \
+      vic_input_data.nullspace_desired_joint_positions - vic_input_data.joint_state_position;
+    // Add nullspace contribution to joint velocity
+    vic_command_data.joint_command_velocity += nullspace_projection_ * D_inv_nullspace_ * (
+      K_nullspace_ * error_position_nullspace
+      + external_joint_torques_);
+  } else {
+    // Pure (small) damping in nullspace for stability
+    RCLCPP_WARN_THROTTLE(
+      logger_,
+      internal_clock_,
+      10000,  // every 10 seconds
+      "WARNING! nullspace impedance control is disabled!"
+    );
+  }
+
+  std::cout << "ref vel: " << reference_compliant_frame.velocity.transpose() << std::endl;
+  std::cout << "cur vel: " << vic_input_data.robot_current_velocity.transpose() << std::endl;
+  std::cout << "ref acc: " << reference_compliant_frame.acceleration.transpose() << std::endl;
+  std::cout << "est acc: " << vic_input_data.robot_estimated_acceleration.transpose() << std::endl;
+  std::cout << "ref wrench: " << reference_compliant_frame.wrench.transpose() << std::endl;
+  std::cout << "D.inv(): " << D.inverse() << std::endl;
+  std::cout << "K: " << K << std::endl;
+  std::cout << "M: " << M << std::endl;
+  std::cout << "robot_command_twist: " << robot_command_twist.transpose() << std::endl;
   */
 
+  // Alternative: use joint accelerations to integrate cartesian velocity
   // Compute joint command accelerations
+
+  // -> commanded_acc = p_ddot_desired + inv(M) * (K * err_p + D * err_p_dot - F_ext + F_ref)
+  // Implement "normal" impedance control
+  RCLCPP_DEBUG(logger_, "Cmd cartesian acc...");
+  Eigen::Matrix<double, 6, 1> commanded_cartesian_acc = reference_compliant_frame.acceleration + \
+    M_inv * (K * error_pose + D * error_velocity - F_ext + reference_compliant_frame.wrench);
+
+  // TODO(tpoignonec): clip cartesian acceleration in min/max range
+  // (and same for velocity if possible)
+
   RCLCPP_DEBUG(logger_, "Cmd joint acc...");
   vic_command_data.joint_command_acceleration = \
     J_pinv_ * (commanded_cartesian_acc - J_dot_ * vic_input_data.joint_state_velocity);
-
   // Nullspace objective for stability
   // ------------------------------------------------
   if (vic_input_data.activate_nullspace_control) {
@@ -274,6 +328,71 @@ bool VanillaCartesianAdmittanceRule::compute_controls(
   vic_command_data.joint_command_velocity += \
     vic_command_data.joint_command_acceleration * dt;
 
+  // Detect and handle singularity issues
+  /*
+  // Singularity detection
+  // from https://github.com/moveit/moveit2/blob/8a0c655e2ba48e1f93f551cd52fb5aa093021659/moveit_ros/moveit_servo/src/utils/common.cpp#L282
+  double singularity_step_scale = 0.01;
+  double hard_stop_singularity_threshold = 30.0;
+  double lower_singularity_threshold = 17.0;
+  double leaving_singularity_threshold_multiplier = 1.5;
+
+  Eigen::VectorXd vector_towards_singularity = J_svd.matrixU().col(dims - 1);
+  const double current_condition_number = J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1);
+  const Eigen::VectorXd delta_x = vector_towards_singularity * singularity_step_scale;
+  // Compute the new joint angles if we take the small step delta_x
+  Eigen::VectorXd next_joint_position = vic_input_data.joint_state_position;
+  next_joint_position += J_pinv_ * delta_x;
+
+   // Compute the Jacobian SVD for the new robot state.
+  Eigen::Matrix<double, 6, Eigen::Dynamic> J_next = Eigen::Matrix<double, 6, Eigen::Dynamic>::Zero(6, dims);
+  dynamics_->calculate_jacobian(
+    next_joint_position,
+    vic_input_data.control_frame,
+    J_next
+  );
+  const Eigen::JacobiSVD<Eigen::MatrixXd> next_svd = Eigen::JacobiSVD<Eigen::MatrixXd>(
+      J_next, Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+  // Compute condition number for the new Jacobian.
+  const double next_condition_number = next_svd.singularValues()(0) / next_svd.singularValues()(dims - 1);
+  if (next_condition_number <= current_condition_number) {
+    vector_towards_singularity *= -1;
+  }
+  // Double check the direction using dot product.
+  const bool moving_towards_singularity = vector_towards_singularity.dot(target_delta_x) > 0;
+
+  // Compute upper condition variable threshold based on if we are moving towards or away from singularity.
+  // See https://github.com/moveit/moveit2/pull/620#issuecomment-1201418258 for visual explanation.
+  double upper_threshold;
+  if (moving_towards_singularity)
+  {
+    upper_threshold = hard_stop_singularity_threshold;
+  }
+  else
+  {
+    const double threshold_size = (hard_stop_singularity_threshold - lower_singularity_threshold);
+    upper_threshold = lower_singularity_threshold + (threshold_size * leaving_singularity_threshold_multiplier);
+  }
+
+  // Compute the scale based on the current condition number.
+  double velocity_scale = 1.0;
+  const bool is_above_lower_limit = current_condition_number > lower_singularity_threshold;
+  const bool is_below_hard_stop_limit = current_condition_number < hard_stop_singularity_threshold;
+  if (is_above_lower_limit && is_below_hard_stop_limit)
+  {
+    velocity_scale -=
+        (current_condition_number - lower_singularity_threshold) / (upper_threshold - lower_singularity_threshold);
+  }
+  // If condition number has crossed hard stop limit, halt the robot.
+  else if (!is_below_hard_stop_limit)
+  {
+    success = false;
+    velocity_scale = 0.0;
+  }
+  */
+
+
   // Filter joint command velocity
   double cutoff_freq_cmd = parameters_.filters.command_filter_cuttoff_freq;
   if (cutoff_freq_cmd > 0.0) {  // No smoothing otherwise