Skip to content

Commit

Permalink
refractor rules
Browse files Browse the repository at this point in the history
  • Loading branch information
Thibault Poignonec committed Aug 27, 2024
1 parent 12dc6b1 commit 86f621c
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 57 deletions.
50 changes: 21 additions & 29 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,12 +186,15 @@ 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_, 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) {
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)!",
Expand Down
50 changes: 22 additions & 28 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 @@ -172,12 +163,15 @@ bool VanillaCartesianImpedanceRule::compute_controls(
Eigen::JacobiSVD<Eigen::MatrixXd>(J_, Eigen::ComputeThinU | Eigen::ComputeThinV);
double conditioning_J = 1000.0;
if (J_.cols() < 6) {
RCLCPP_WARN_THROTTLE(
logger_, 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) {
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)!",
Expand Down

0 comments on commit 86f621c

Please sign in to comment.