Skip to content

Commit

Permalink
minor fixes rules
Browse files Browse the repository at this point in the history 
(cherry picked from commit 2c330d6)
  • Loading branch information
Thibault Poignonec committed Aug 28, 2024
1 parent 1768081 commit 01aa352
Show file tree
Hide file tree
Showing 4 changed files with 68 additions and 61 deletions.
2 changes: 1 addition & 1 deletion ...c_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_admittance_rule.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ class VanillaCartesianAdmittanceRule : public CartesianVicRule
Eigen::Matrix<double, Eigen::Dynamic, 6> J_pinv_;
Eigen::Matrix<double, 6, Eigen::Dynamic> J_dot_;

double alpha_pinv_ = 0.0005;
double alpha_pinv_ = 0.001;

// Nullspace solver
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> nullspace_projection_;
Expand Down
2 changes: 1 addition & 1 deletion ...ic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_impedance_rule.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ class VanillaCartesianImpedanceRule : public CartesianVicRule

Eigen::VectorXd raw_joint_command_effort_;

double alpha_pinv_ = 0.0005;
double alpha_pinv_ = 0.001;

// Nullspace solver
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> nullspace_projection_;
Expand Down
62 changes: 32 additions & 30 deletions cartesian_vic_controller/src/rules/vanilla_cartesian_admittance_rule.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,35 +97,26 @@ bool VanillaCartesianAdmittanceRule::compute_controls(
auto rot_base_admittance = vic_transforms_.base_vic_.rotation();

// Express M, K, D matrices in base (provided in base_vic frame)
auto registration_MKD = [&rot_base_admittance](
const Eigen::Matrix<double, 6, 6> & matrix_in_adm_frame,
Eigen::Matrix<double, 6, 6> & matrix_in_base_frame)
{
matrix_in_base_frame.block<3, 3>(0, 0) =
rot_base_admittance * \
matrix_in_adm_frame.block<3, 3>(0, 0) * \
rot_base_admittance.transpose();
matrix_in_base_frame.block<3, 3>(3, 3) =
rot_base_admittance * \
matrix_in_adm_frame.block<3, 3>(3, 3) * \
rot_base_admittance.transpose();
return;
};
Eigen::Matrix<double, 6, 6> M = Eigen::Matrix<double, 6, 6>::Zero();
Eigen::Matrix<double, 6, 6> K = Eigen::Matrix<double, 6, 6>::Zero();
K.block<3, 3>(0, 0) =
rot_base_admittance * \
vic_command_data.stiffness.block<3, 3>(0, 0) * \
rot_base_admittance.transpose();
K.block<3, 3>(3, 3) =
rot_base_admittance * \
vic_command_data.stiffness.block<3, 3>(3, 3) * \
rot_base_admittance.transpose();

Eigen::Matrix<double, 6, 6> D = Eigen::Matrix<double, 6, 6>::Zero();
D.block<3, 3>(0, 0) =
rot_base_admittance * \
vic_command_data.damping.block<3, 3>(0, 0) * \
rot_base_admittance.transpose();
D.block<3, 3>(3, 3) =
rot_base_admittance * \
vic_command_data.damping.block<3, 3>(3, 3) * \
rot_base_admittance.transpose();

Eigen::Matrix<double, 6, 6> M = Eigen::Matrix<double, 6, 6>::Zero();
M.block<3, 3>(0, 0) =
rot_base_admittance * \
vic_command_data.inertia.block<3, 3>(0, 0) * \
rot_base_admittance.transpose();
M.block<3, 3>(3, 3) =
rot_base_admittance * \
vic_command_data.inertia.block<3, 3>(3, 3) * \
rot_base_admittance.transpose();
registration_MKD(vic_command_data.inertia, M);
registration_MKD(vic_command_data.stiffness, K);
registration_MKD(vic_command_data.damping, D);

// Compute pose tracking errors
Eigen::Matrix<double, 6, 1> error_pose;
Expand All @@ -137,15 +128,13 @@ bool VanillaCartesianAdmittanceRule::compute_controls(
reference_compliant_frame.pose.rotation() * \
vic_input_data.robot_current_pose.rotation().transpose();
auto angle_axis = Eigen::AngleAxisd(R_angular_error);

error_pose.tail(3) = angle_axis.angle() * angle_axis.axis();

// Compute velocity tracking errors in ft frame
Eigen::Matrix<double, 6, 1> error_velocity =
reference_compliant_frame.velocity - \
vic_input_data.robot_current_velocity;


// Retrieve forces if needed (not used if use_natural_robot_inertia is set to True)
// RQ: external force at interaction frame (assumed to be control frame),
// expressed in the base frame
Expand Down Expand Up @@ -197,17 +186,30 @@ bool VanillaCartesianAdmittanceRule::compute_controls(
Eigen::JacobiSVD<Eigen::MatrixXd>(J_, Eigen::ComputeThinU | Eigen::ComputeThinV);
double conditioning_J = 1000.0;
if (J_.cols() < 6) {
RCLCPP_WARN_THROTTLE(
logger_, internal_clock_, 5000, "Jacobian has only %u columns, expecting at least 6!!!",
J_.cols());
conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(J_.cols() - 1);
} else {
conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1);
}
if (conditioning_J > 30) {
if (conditioning_J > 40) {
success = false;
std::cerr << "J_svd.singularValues() = " << J_svd.singularValues().transpose() << std::endl;
RCLCPP_ERROR(
logger_,
"Jacobian singularity detected (max(singular values)/min(singular values) = %lf)!",
conditioning_J
);
} else if (conditioning_J > 15) {
RCLCPP_WARN_THROTTLE(
logger_,
internal_clock_,
5000,
"Nearing Jacobian singularity (max(singular values)/min(singular values) = %lf), "
"proceed with caution!",
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();
Expand Down
63 changes: 34 additions & 29 deletions cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,37 +80,28 @@ bool VanillaCartesianImpedanceRule::compute_controls(

// auto rot_base_control = vic_transforms_.base_control_.rotation();
auto rot_base_impedance = vic_transforms_.base_vic_.rotation();
// Express M, K, D matrices in base (provided in base_vic frame)

// Express M, K, D matrices in base (provided in base_vic frame)
auto registration_MKD = [&rot_base_impedance](
const Eigen::Matrix<double, 6, 6> & matrix_in_adm_frame,
Eigen::Matrix<double, 6, 6> & matrix_in_base_frame)
{
matrix_in_base_frame.block<3, 3>(0, 0) =
rot_base_impedance * \
matrix_in_adm_frame.block<3, 3>(0, 0) * \
rot_base_impedance.transpose();
matrix_in_base_frame.block<3, 3>(3, 3) =
rot_base_impedance * \
matrix_in_adm_frame.block<3, 3>(3, 3) * \
rot_base_impedance.transpose();
return;
};
Eigen::Matrix<double, 6, 6> M = Eigen::Matrix<double, 6, 6>::Zero();
Eigen::Matrix<double, 6, 6> K = Eigen::Matrix<double, 6, 6>::Zero();
K.block<3, 3>(0, 0) =
rot_base_impedance * \
vic_command_data.stiffness.block<3, 3>(0, 0) * \
rot_base_impedance.transpose();
K.block<3, 3>(3, 3) =
rot_base_impedance * \
vic_command_data.stiffness.block<3, 3>(3, 3) * \
rot_base_impedance.transpose();

Eigen::Matrix<double, 6, 6> D = Eigen::Matrix<double, 6, 6>::Zero();
D.block<3, 3>(0, 0) =
rot_base_impedance * \
vic_command_data.damping.block<3, 3>(0, 0) * \
rot_base_impedance.transpose();
D.block<3, 3>(3, 3) =
rot_base_impedance * \
vic_command_data.damping.block<3, 3>(3, 3) * \
rot_base_impedance.transpose();

Eigen::Matrix<double, 6, 6> M = Eigen::Matrix<double, 6, 6>::Zero();
M.block<3, 3>(0, 0) =
rot_base_impedance * \
vic_command_data.inertia.block<3, 3>(0, 0) * \
rot_base_impedance.transpose();
M.block<3, 3>(3, 3) =
rot_base_impedance * \
vic_command_data.inertia.block<3, 3>(3, 3) * \
rot_base_impedance.transpose();
registration_MKD(vic_command_data.inertia, M);
registration_MKD(vic_command_data.stiffness, K);
registration_MKD(vic_command_data.damping, D);

// Compute pose tracking errors
Eigen::Matrix<double, 6, 1> error_pose;
Expand Down Expand Up @@ -167,22 +158,36 @@ bool VanillaCartesianImpedanceRule::compute_controls(
vic_input_data.end_effector_frame,
J_dot_
);

RCLCPP_DEBUG(logger_, "Computing J_pinv...");
const Eigen::JacobiSVD<Eigen::MatrixXd> J_svd =
Eigen::JacobiSVD<Eigen::MatrixXd>(J_, Eigen::ComputeThinU | Eigen::ComputeThinV);
double conditioning_J = 1000.0;
if (J_.cols() < 6) {
RCLCPP_WARN_THROTTLE(
logger_, internal_clock_, 5000, "Jacobian has only %u columns, expecting at least 6!!!",
J_.cols());
conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(J_.cols() - 1);
} else {
conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1);
}
if (conditioning_J > 30) {
if (conditioning_J > 40) {
success = false;
std::cerr << "J_svd.singularValues() = " << J_svd.singularValues().transpose() << std::endl;
RCLCPP_ERROR(
logger_,
"Jacobian singularity detected (max(singular values)/min(singular values) = %lf)!",
conditioning_J
);
} else if (conditioning_J > 15) {
RCLCPP_WARN_THROTTLE(
logger_,
internal_clock_,
5000,
"Nearing Jacobian singularity (max(singular values)/min(singular values) = %lf), "
"proceed with caution!",
conditioning_J
);
}
J_pinv_ = (J_.transpose() * J_ + alpha_pinv_ * I_joint_space_).inverse() * J_.transpose();

Expand Down

0 comments on commit 01aa352

Please sign in to comment.