From 80d1422e93dfa7ae7d0e25fff576540306eed586 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 23 Aug 2024 12:34:14 +0200 Subject: [PATCH] fix adm control (#32) (#33) * fix adm rule * revert impedance rule to normal inverse() (instead of LLT-based inverses) * increase alpha pinv * check for singularities in impedance control * prepare admittance for singularity checks * add commented alternative method for acc estimation * fix missing var in impedance rule * change start position * use "end_effector_frame" instead of control frame (cherry picked from commit 5921f9aaef23a03b8393b7eb30cd0123328ecf73) Co-authored-by: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> --- .../example_admittance_controller_config.yaml | 4 +- .../example_impedance_controller_config.yaml | 2 +- .../cartesian_vic_rule.hpp | 3 + .../cartesian_vic_state.hpp | 27 ++- .../vanilla_cartesian_admittance_rule.hpp | 2 +- .../vanilla_cartesian_impedance_rule.hpp | 2 +- .../cartesian_vic_controller_parameters.yaml | 6 +- .../src/cartesian_vic_rule.cpp | 39 ++-- .../src/cartesian_vic_state.cpp | 2 + .../vanilla_cartesian_admittance_rule.cpp | 170 +++++++++++++++--- .../vanilla_cartesian_impedance_rule.cpp | 29 ++- .../test/test_cartesian_vic_controller.hpp | 3 +- 12 files changed, 217 insertions(+), 72 deletions(-) diff --git a/cartesian_vic_controller/config/example_admittance_controller_config.yaml b/cartesian_vic_controller/config/example_admittance_controller_config.yaml index fa084dc..597f1e4 100644 --- a/cartesian_vic_controller/config/example_admittance_controller_config.yaml +++ b/cartesian_vic_controller/config/example_admittance_controller_config.yaml @@ -42,9 +42,9 @@ cartesian_impedance_controller: is_enabled: false name: "" - control: + end_effector_frame: frame: - id: interaction_point # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + id: interaction_point external: false # control frame exists within URDF kinematic chain fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) diff --git a/cartesian_vic_controller/config/example_impedance_controller_config.yaml b/cartesian_vic_controller/config/example_impedance_controller_config.yaml index b3ae153..9b6bb64 100644 --- a/cartesian_vic_controller/config/example_impedance_controller_config.yaml +++ b/cartesian_vic_controller/config/example_impedance_controller_config.yaml @@ -41,7 +41,7 @@ cartesian_impedance_controller: is_enabled: false name: "" - control: + end_effector_frame: frame: id: interaction_point # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector external: false # control frame exists within URDF kinematic chain diff --git a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_rule.hpp b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_rule.hpp index 8c4a0b9..c4bb236 100644 --- a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_rule.hpp +++ b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_rule.hpp @@ -206,6 +206,9 @@ class CartesianVicRule /// Jacobian pre-allocation (used to compute cartesian inertia matrix) Eigen::Matrix J_private_; + /// Jacobian pre-allocation (used to compute cartesian inertia matrix) + // Eigen::Matrix J_dot_private_; + /// Filtered external torques Eigen::VectorXd filtered_external_torques_; }; diff --git a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp index d9aa72d..05625ab 100644 --- a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp +++ b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp @@ -127,12 +127,12 @@ class VicInputData bool activate_nullspace_control = false; bool activate_gravity_compensation = false; - /// Name of the robot base frame + /// Name of the control frame in which is expressed the cartesian pose/vel/acc/wrench reference std::string base_frame; /// Name of the compliance (i.e., vic) frame in which compliance parameters are specified std::string vic_frame; - /// Name of the control frame in which is expressed the cartesian pose/vel/acc/wrench reference - std::string control_frame; + /// Name of the controlled robot end-effector frame + std::string end_effector_frame; /// Name of the force/torque sensor frame in which is expressed the measured wrench std::string ft_sensor_frame; @@ -236,23 +236,16 @@ class VicState /// Transforms between frames used in the Vic controller struct VicTransforms { - // 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 + vic - // offset joint angles + // transformation from fixed world frame to base link frame + Eigen::Isometry3d world_base_; + // transformation from force torque sensor frame to base link frame Eigen::Isometry3d base_ft_; - // transformation from control frame to base link frame at reference + vic offset joint - // angles - Eigen::Isometry3d base_control_; + // transformation from controlled end effector (EE) frame to base link frame + Eigen::Isometry3d base_end_effector_; + // transformation from vic frame to base link frame Eigen::Isometry3d base_vic_; - // transformation from end effector frame to base link frame at reference + vic offset - // joint angles - Eigen::Isometry3d base_tip_; - // transformation from center of gravity frame to base link frame at reference + vic offset - // joint angles + // transformation from CoG frame (i.e., post-sensor inertial frame) to base link frame Eigen::Isometry3d base_cog_; - // transformation from world frame to base link frame - Eigen::Isometry3d world_base_; }; diff --git a/cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_admittance_rule.hpp b/cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_admittance_rule.hpp index 49c5507..46a0a76 100644 --- a/cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_admittance_rule.hpp +++ b/cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_admittance_rule.hpp @@ -58,7 +58,7 @@ class VanillaCartesianAdmittanceRule : public CartesianVicRule Eigen::Matrix J_pinv_; Eigen::Matrix J_dot_; - double alpha_pinv_ = 0.000005; + double alpha_pinv_ = 0.0005; // Nullspace solver Eigen::Matrix nullspace_projection_; diff --git a/cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_impedance_rule.hpp b/cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_impedance_rule.hpp index d86af74..9843374 100644 --- a/cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_impedance_rule.hpp +++ b/cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_impedance_rule.hpp @@ -63,7 +63,7 @@ class VanillaCartesianImpedanceRule : public CartesianVicRule Eigen::VectorXd raw_joint_command_effort_; - double alpha_pinv_ = 0.000005; + double alpha_pinv_ = 0.0005; // Nullspace solver Eigen::Matrix nullspace_projection_; diff --git a/cartesian_vic_controller/src/cartesian_vic_controller_parameters.yaml b/cartesian_vic_controller/src/cartesian_vic_controller_parameters.yaml index 3ca7f3d..39565e5 100644 --- a/cartesian_vic_controller/src/cartesian_vic_controller_parameters.yaml +++ b/cartesian_vic_controller/src/cartesian_vic_controller_parameters.yaml @@ -111,11 +111,11 @@ cartesian_vic_controller: description: "Specifies the name of the external torque sensor in the robot description which will be used in the vic calculation." } - control: + end_effector_frame: frame: id: { type: string, - description: "Specifies the robot control frame in which the reference pose and velocities are expressed." + description: "Specifies the controlled robot end effector frame. Note that the pose and velocity are expressed w.r.t. the base frame (see dynamics plugin parameters)." } fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) @@ -149,7 +149,7 @@ cartesian_vic_controller: frame: id: { type: string, - description: "Specifies the control frame used for vic calculation." + description: "Specifies the frame in which the impedance matrices are expressed." } plugin_name: { type: string, diff --git a/cartesian_vic_controller/src/cartesian_vic_rule.cpp b/cartesian_vic_controller/src/cartesian_vic_rule.cpp index 8cdc0f6..cc559c4 100644 --- a/cartesian_vic_controller/src/cartesian_vic_rule.cpp +++ b/cartesian_vic_controller/src/cartesian_vic_rule.cpp @@ -94,6 +94,9 @@ CartesianVicRule::configure( J_private_ = \ Eigen::Matrix::Zero(6, num_joints); + // J_dot_private_ = \ + // Eigen::Matrix::Zero(6, num_joints); + return controller_interface::return_type::OK; } @@ -191,7 +194,7 @@ void CartesianVicRule::apply_parameters_update() parameters_.vic.activate_gravity_compensation; vic_state_.input_data.vic_frame = parameters_.vic.frame.id; - vic_state_.input_data.control_frame = parameters_.control.frame.id; + vic_state_.input_data.end_effector_frame = parameters_.end_effector_frame.frame.id; vic_state_.input_data.ft_sensor_frame = parameters_.ft_sensor.frame.id; if (!use_streamed_interaction_parameters_) { @@ -318,7 +321,7 @@ CartesianVicRule::update_compliant_frame_trajectory( // Update reference compliant frames bool success = true; for (unsigned int i = 0; i < N; i++) { - // TODO(tpoignonec): Check the frame is correct (i.e., control w.r.t. base)! + // TODO(tpoignonec): Check the frame is correct (i.e., end-effector w.r.t. base)! success &= \ vic_state_.input_data.reference_compliant_frames.fill_desired_robot_state_from_msg( i, @@ -449,7 +452,7 @@ CartesianVicRule::update_input_data( ); success &= dynamics_->calculate_jacobian( vic_state_.input_data.joint_state_position, - vic_state_.input_data.control_frame, + vic_state_.input_data.end_effector_frame, J_private_ ); vic_state_.input_data.natural_cartesian_inertia = (J_private_ * \ @@ -571,6 +574,7 @@ bool CartesianVicRule::update_kinematics( } // Update current robot joint velocity + // auto previous_joint_velocity = vic_state_.input_data.joint_state_velocity; double cutoff_jnt_velocity = parameters_.filters.state_velocity_filter_cuttoff_freq; if (dt > 0 && cutoff_jnt_velocity > 0.0) { double jnt_velocity_filter_coefficient = 1.0 - exp(-dt * 2 * 3.14 * cutoff_jnt_velocity); @@ -589,7 +593,7 @@ bool CartesianVicRule::update_kinematics( // Update current cartesian pose and velocity from robot joint states success &= dynamics_->calculate_link_transform( vic_state_.input_data.joint_state_position, - vic_state_.input_data.control_frame, + vic_state_.input_data.end_effector_frame, vic_state_.input_data.robot_current_pose ); @@ -597,7 +601,7 @@ bool CartesianVicRule::update_kinematics( success = dynamics_->convert_joint_deltas_to_cartesian_deltas( vic_state_.input_data.joint_state_position, vic_state_.input_data.joint_state_velocity, - vic_state_.input_data.control_frame, + vic_state_.input_data.end_effector_frame, vic_state_.input_data.robot_current_velocity ); @@ -606,6 +610,17 @@ bool CartesianVicRule::update_kinematics( if (dt > 0) { auto raw_acc = \ (vic_state_.input_data.robot_current_velocity - last_robot_cartesian_velocity) / dt; + /* + auto joint_acc = (vic_state_.input_data.joint_state_velocity - previous_joint_velocity) / dt; + success &= dynamics_->calculate_jacobian_derivative( + vic_state_.input_data.joint_state_position, + vic_state_.input_data.joint_state_velocity, + vic_state_.input_data.end_effector_frame, + J_dot_private_ + ); + auto raw_acc = J_private_ * joint_acc + J_dot_private_ * + vic_state_.input_data.joint_state_velocity; + */ if (cutoff_acceleration > 0.0) { double cutoff_acceleration = 30.0; // Hz double acceleration_filter_coefficient = 1.0 - exp(-dt * 2 * 3.14 * cutoff_acceleration); @@ -635,12 +650,6 @@ bool CartesianVicRule::update_kinematics( // In theory, there is no reason to use it if no wrench is available, but you never know... vic_transforms_.base_ft_.setIdentity(); } - - success &= dynamics_->calculate_link_transform( - vic_state_.input_data.joint_state_position, - parameters_.dynamics.tip, - vic_transforms_.base_tip_ - ); success &= dynamics_->calculate_link_transform( vic_state_.input_data.joint_state_position, parameters_.fixed_world_frame.frame.id, @@ -653,12 +662,12 @@ bool CartesianVicRule::update_kinematics( ); success &= dynamics_->calculate_link_transform( vic_state_.input_data.joint_state_position, - parameters_.control.frame.id, - vic_transforms_.base_control_ + parameters_.end_effector_frame.frame.id, + vic_transforms_.base_end_effector_ ); success &= dynamics_->calculate_link_transform( vic_state_.input_data.joint_state_position, - parameters_.control.frame.id, + parameters_.vic.frame.id, vic_transforms_.base_vic_ ); return true; @@ -694,7 +703,7 @@ bool CartesianVicRule::process_wrench_measurements( new_wrench_world.block<3, 1>(0, 1) -= (rot_world_cog * cog_pos).cross(end_effector_weight); /* - // Wrench at interaction point (e.g., assumed to be control frame + // Wrench at interaction point (assumed to be end-effector frame) F_ext.block<3, 1>(0, 0) = rot_base_control.transpose() * F_ext_base.block<3, 1>(0, 0); // Evaluate torques at new interaction point F_ext.block<3, 1>(3, 0) = rot_base_control.transpose() * ( diff --git a/cartesian_vic_controller/src/cartesian_vic_state.cpp b/cartesian_vic_controller/src/cartesian_vic_state.cpp index fef72b0..6da9503 100644 --- a/cartesian_vic_controller/src/cartesian_vic_state.cpp +++ b/cartesian_vic_controller/src/cartesian_vic_state.cpp @@ -54,6 +54,8 @@ VicState::to_msg(cartesian_control_msgs::msg::VicControllerState & vic_state_msg success = false; } + // Fill frame names + // Fill desired compliance auto desired_frame_0 = \ input_data.reference_compliant_frames.get_compliant_frame(0); diff --git a/cartesian_vic_controller/src/rules/vanilla_cartesian_admittance_rule.cpp b/cartesian_vic_controller/src/rules/vanilla_cartesian_admittance_rule.cpp index 9e7f193..da07bf3 100644 --- a/cartesian_vic_controller/src/rules/vanilla_cartesian_admittance_rule.cpp +++ b/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..."); @@ -86,6 +89,10 @@ bool VanillaCartesianAdmittanceRule::compute_controls( // -------------------------------- RCLCPP_DEBUG(logger_, "Preparing data..."); + + // Copy previous command velocity (used for integration) + auto previous_jnt_cmd_velocity = vic_command_data.joint_command_velocity; + // auto rot_base_control = vic_transforms_.base_control_.rotation(); auto rot_base_admittance = vic_transforms_.base_vic_.rotation(); @@ -166,19 +173,16 @@ bool VanillaCartesianAdmittanceRule::compute_controls( RCLCPP_DEBUG(logger_, "computing J and J_dot..."); bool model_is_ok = dynamics_->calculate_jacobian( vic_input_data.joint_state_position, - vic_input_data.control_frame, + vic_input_data.end_effector_frame, J_ ); model_is_ok &= dynamics_->calculate_jacobian_derivative( vic_input_data.joint_state_position, vic_input_data.joint_state_velocity, - vic_input_data.control_frame, + vic_input_data.end_effector_frame, J_dot_ ); - RCLCPP_DEBUG(logger_, "Computing J_pinv..."); - J_pinv_ = (J_.transpose() * J_ + alpha_pinv_ * I_joint_space_).llt().solve(I_) * J_.transpose(); - if (!model_is_ok) { success = false; RCLCPP_ERROR( @@ -187,12 +191,29 @@ bool VanillaCartesianAdmittanceRule::compute_controls( ); } + + RCLCPP_DEBUG(logger_, "Computing J_pinv..."); + const Eigen::JacobiSVD J_svd = + Eigen::JacobiSVD(J_, Eigen::ComputeThinU | Eigen::ComputeThinV); + double conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1); + if (conditioning_J > 30) { + success = false; + RCLCPP_ERROR( + logger_, + "Jacobian singularity detected (max(singular values)/min(singular values) = %lf)!", + conditioning_J + ); + } + // 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 M_inv; if (parameters_.vic.use_natural_robot_inertia) { M = vic_input_data.natural_cartesian_inertia; - M_inv = vic_input_data.natural_cartesian_inertia.llt().solve(I_); + // M_inv = vic_input_data.natural_cartesian_inertia.llt().solve(I_); + M_inv = vic_input_data.natural_cartesian_inertia.inverse(); RCLCPP_INFO_THROTTLE( logger_, internal_clock_, @@ -200,7 +221,8 @@ bool VanillaCartesianAdmittanceRule::compute_controls( "Using natural robot inertia as desired inertia matrix." ); } else { - M_inv = M.llt().solve(I_); + // M_inv = M.llt().solve(I_); + M_inv = M.inverse(); } // Compute admittance control law in the base frame @@ -208,37 +230,74 @@ 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 - Eigen::Matrix commanded_cartesian_acc = - reference_compliant_frame.acceleration + \ - M_inv * (K * error_pose + D * error_velocity - F_ext + reference_compliant_frame.wrench); - // Copy previous command velocity (used for integration) - auto previous_jnt_cmd_velocity = vic_command_data.joint_command_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_input_data.end_effector_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 commanded_cartesian_acc = reference_compliant_frame.acceleration + \ M_inv * (K * error_pose + D * error_velocity - F_ext + reference_compliant_frame.wrench); - // Compute joint command accelerations + // 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) { RCLCPP_DEBUG(logger_, "Cmd nullspace joint acc..."); nullspace_projection_ = I_joint_space_ - J_pinv_ * J_; @@ -251,8 +310,8 @@ bool VanillaCartesianAdmittanceRule::compute_controls( // Add nullspace contribution to joint accelerations vic_command_data.joint_command_acceleration += nullspace_projection_ * M_inv_nullspace_ * ( -D_nullspace_ * vic_input_data.joint_state_velocity + - K_nullspace_ * error_position_nullspace - + external_joint_torques_ + K_nullspace_ * error_position_nullspace + + external_joint_torques_ ); } else { // Pure (small) damping in nullspace for stability @@ -269,6 +328,69 @@ bool VanillaCartesianAdmittanceRule::compute_controls( RCLCPP_DEBUG(logger_, "Integration acc..."); 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 J_next = Eigen::Matrix::Zero(6, dims); + dynamics_->calculate_jacobian( + next_joint_position, + vic_input_data.end_effector_frame, + J_next + ); + const Eigen::JacobiSVD next_svd = Eigen::JacobiSVD( + 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; + } */ diff --git a/cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp b/cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp index b43922a..341743d 100644 --- a/cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp +++ b/cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp @@ -55,7 +55,9 @@ bool VanillaCartesianImpedanceRule::compute_controls( VicCommandData & vic_command_data) { bool success = true; - // auto num_joints = vic_input_data.joint_state_position.size(); + + size_t num_joints = vic_state_.input_data.joint_state_position.size(); + size_t dims = 6; // 6 DoF if (dt <= 0.0) { RCLCPP_ERROR(logger_, "Sampling time should be positive, received %f", dt); @@ -156,17 +158,28 @@ bool VanillaCartesianImpedanceRule::compute_controls( RCLCPP_DEBUG(logger_, "computing J and J_dot..."); bool model_is_ok = dynamics_->calculate_jacobian( vic_input_data.joint_state_position, - vic_input_data.control_frame, + vic_input_data.end_effector_frame, J_ ); model_is_ok &= dynamics_->calculate_jacobian_derivative( vic_input_data.joint_state_position, vic_input_data.joint_state_velocity, - vic_input_data.control_frame, + vic_input_data.end_effector_frame, J_dot_ ); RCLCPP_DEBUG(logger_, "Computing J_pinv..."); - J_pinv_ = (J_.transpose() * J_ + alpha_pinv_ * I_joint_space_).llt().solve(I_) * J_.transpose(); + const Eigen::JacobiSVD J_svd = + Eigen::JacobiSVD(J_, Eigen::ComputeThinU | Eigen::ComputeThinV); + double conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1); + if (conditioning_J > 30) { + success = false; + RCLCPP_ERROR( + logger_, + "Jacobian singularity detected (max(singular values)/min(singular values) = %lf)!", + conditioning_J + ); + } + J_pinv_ = (J_.transpose() * J_ + alpha_pinv_ * I_joint_space_).inverse() * J_.transpose(); if (!model_is_ok) { success = false; @@ -181,7 +194,8 @@ bool VanillaCartesianImpedanceRule::compute_controls( Eigen::Matrix M_inv; if (parameters_.vic.use_natural_robot_inertia) { M = vic_input_data.natural_cartesian_inertia; - M_inv = vic_input_data.natural_cartesian_inertia.llt().solve(I_); + // M_inv = vic_input_data.natural_cartesian_inertia.llt().solve(I_); + M_inv = vic_input_data.natural_cartesian_inertia.inverse(); RCLCPP_INFO_THROTTLE( logger_, internal_clock_, @@ -189,7 +203,8 @@ bool VanillaCartesianImpedanceRule::compute_controls( "Using natural robot inertia as desired inertia matrix." ); } else { - M_inv = M.llt().solve(I_); + // M_inv = M.llt().solve(I_); + M_inv = M.inverse(); } @@ -295,7 +310,7 @@ bool VanillaCartesianImpedanceRule::compute_controls( logger_, internal_clock_, 10000, // every 10 seconds - "WARNING! gravity compensation is disabled!" + "FYI: gravity compensation is disabled." ); } diff --git a/cartesian_vic_controller/test/test_cartesian_vic_controller.hpp b/cartesian_vic_controller/test/test_cartesian_vic_controller.hpp index 7a28d46..9a87f76 100644 --- a/cartesian_vic_controller/test/test_cartesian_vic_controller.hpp +++ b/cartesian_vic_controller/test/test_cartesian_vic_controller.hpp @@ -437,7 +437,8 @@ class CartesianVicControllerTest : public ::testing::Test std::array vic_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6}; std::array joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - std::array joint_state_position_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; + std::array joint_state_position_values_ = {0.0, 0.1, 0.2, 0.3, 0.4, 0.5}; + std::array joint_state_velocity_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; std::vector fts_state_names_;