Skip to content

Commit 01aa352

Browse files
author
Thibault Poignonec
committed
minor fixes rules
(cherry picked from commit 2c330d6)
1 parent 1768081 commit 01aa352

File tree

4 files changed

+68
-61
lines changed

4 files changed

+68
-61
lines changed

‎cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_admittance_rule.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,7 @@ class VanillaCartesianAdmittanceRule : public CartesianVicRule
5858
Eigen::Matrix<double, Eigen::Dynamic, 6> J_pinv_;
5959
Eigen::Matrix<double, 6, Eigen::Dynamic> J_dot_;
6060

61-
double alpha_pinv_ = 0.0005;
61+
double alpha_pinv_ = 0.001;
6262

6363
// Nullspace solver
6464
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> nullspace_projection_;

‎cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_impedance_rule.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ class VanillaCartesianImpedanceRule : public CartesianVicRule
6363

6464
Eigen::VectorXd raw_joint_command_effort_;
6565

66-
double alpha_pinv_ = 0.0005;
66+
double alpha_pinv_ = 0.001;
6767

6868
// Nullspace solver
6969
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> nullspace_projection_;

‎cartesian_vic_controller/src/rules/vanilla_cartesian_admittance_rule.cpp

Lines changed: 32 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -97,35 +97,26 @@ bool VanillaCartesianAdmittanceRule::compute_controls(
9797
auto rot_base_admittance = vic_transforms_.base_vic_.rotation();
9898

9999
// Express M, K, D matrices in base (provided in base_vic frame)
100+
auto registration_MKD = [&rot_base_admittance](
101+
const Eigen::Matrix<double, 6, 6> & matrix_in_adm_frame,
102+
Eigen::Matrix<double, 6, 6> & matrix_in_base_frame)
103+
{
104+
matrix_in_base_frame.block<3, 3>(0, 0) =
105+
rot_base_admittance * \
106+
matrix_in_adm_frame.block<3, 3>(0, 0) * \
107+
rot_base_admittance.transpose();
108+
matrix_in_base_frame.block<3, 3>(3, 3) =
109+
rot_base_admittance * \
110+
matrix_in_adm_frame.block<3, 3>(3, 3) * \
111+
rot_base_admittance.transpose();
112+
return;
113+
};
114+
Eigen::Matrix<double, 6, 6> M = Eigen::Matrix<double, 6, 6>::Zero();
100115
Eigen::Matrix<double, 6, 6> K = Eigen::Matrix<double, 6, 6>::Zero();
101-
K.block<3, 3>(0, 0) =
102-
rot_base_admittance * \
103-
vic_command_data.stiffness.block<3, 3>(0, 0) * \
104-
rot_base_admittance.transpose();
105-
K.block<3, 3>(3, 3) =
106-
rot_base_admittance * \
107-
vic_command_data.stiffness.block<3, 3>(3, 3) * \
108-
rot_base_admittance.transpose();
109-
110116
Eigen::Matrix<double, 6, 6> D = Eigen::Matrix<double, 6, 6>::Zero();
111-
D.block<3, 3>(0, 0) =
112-
rot_base_admittance * \
113-
vic_command_data.damping.block<3, 3>(0, 0) * \
114-
rot_base_admittance.transpose();
115-
D.block<3, 3>(3, 3) =
116-
rot_base_admittance * \
117-
vic_command_data.damping.block<3, 3>(3, 3) * \
118-
rot_base_admittance.transpose();
119-
120-
Eigen::Matrix<double, 6, 6> M = Eigen::Matrix<double, 6, 6>::Zero();
121-
M.block<3, 3>(0, 0) =
122-
rot_base_admittance * \
123-
vic_command_data.inertia.block<3, 3>(0, 0) * \
124-
rot_base_admittance.transpose();
125-
M.block<3, 3>(3, 3) =
126-
rot_base_admittance * \
127-
vic_command_data.inertia.block<3, 3>(3, 3) * \
128-
rot_base_admittance.transpose();
117+
registration_MKD(vic_command_data.inertia, M);
118+
registration_MKD(vic_command_data.stiffness, K);
119+
registration_MKD(vic_command_data.damping, D);
129120

130121
// Compute pose tracking errors
131122
Eigen::Matrix<double, 6, 1> error_pose;
@@ -137,15 +128,13 @@ bool VanillaCartesianAdmittanceRule::compute_controls(
137128
reference_compliant_frame.pose.rotation() * \
138129
vic_input_data.robot_current_pose.rotation().transpose();
139130
auto angle_axis = Eigen::AngleAxisd(R_angular_error);
140-
141131
error_pose.tail(3) = angle_axis.angle() * angle_axis.axis();
142132

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

148-
149138
// Retrieve forces if needed (not used if use_natural_robot_inertia is set to True)
150139
// RQ: external force at interaction frame (assumed to be control frame),
151140
// expressed in the base frame
@@ -197,17 +186,30 @@ bool VanillaCartesianAdmittanceRule::compute_controls(
197186
Eigen::JacobiSVD<Eigen::MatrixXd>(J_, Eigen::ComputeThinU | Eigen::ComputeThinV);
198187
double conditioning_J = 1000.0;
199188
if (J_.cols() < 6) {
189+
RCLCPP_WARN_THROTTLE(
190+
logger_, internal_clock_, 5000, "Jacobian has only %u columns, expecting at least 6!!!",
191+
J_.cols());
200192
conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(J_.cols() - 1);
201193
} else {
202194
conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1);
203195
}
204-
if (conditioning_J > 30) {
196+
if (conditioning_J > 40) {
205197
success = false;
198+
std::cerr << "J_svd.singularValues() = " << J_svd.singularValues().transpose() << std::endl;
206199
RCLCPP_ERROR(
207200
logger_,
208201
"Jacobian singularity detected (max(singular values)/min(singular values) = %lf)!",
209202
conditioning_J
210203
);
204+
} else if (conditioning_J > 15) {
205+
RCLCPP_WARN_THROTTLE(
206+
logger_,
207+
internal_clock_,
208+
5000,
209+
"Nearing Jacobian singularity (max(singular values)/min(singular values) = %lf), "
210+
"proceed with caution!",
211+
conditioning_J
212+
);
211213
}
212214
// J_pinv_ = J_svd.matrixV() * matrix_s.inverse() * J_svd.matrixU().transpose();
213215
J_pinv_ = (J_.transpose() * J_ + alpha_pinv_ * I_joint_space_).inverse() * J_.transpose();

‎cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp

Lines changed: 34 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -80,37 +80,28 @@ bool VanillaCartesianImpedanceRule::compute_controls(
8080

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

84+
// Express M, K, D matrices in base (provided in base_vic frame)
85+
auto registration_MKD = [&rot_base_impedance](
86+
const Eigen::Matrix<double, 6, 6> & matrix_in_adm_frame,
87+
Eigen::Matrix<double, 6, 6> & matrix_in_base_frame)
88+
{
89+
matrix_in_base_frame.block<3, 3>(0, 0) =
90+
rot_base_impedance * \
91+
matrix_in_adm_frame.block<3, 3>(0, 0) * \
92+
rot_base_impedance.transpose();
93+
matrix_in_base_frame.block<3, 3>(3, 3) =
94+
rot_base_impedance * \
95+
matrix_in_adm_frame.block<3, 3>(3, 3) * \
96+
rot_base_impedance.transpose();
97+
return;
98+
};
99+
Eigen::Matrix<double, 6, 6> M = Eigen::Matrix<double, 6, 6>::Zero();
85100
Eigen::Matrix<double, 6, 6> K = Eigen::Matrix<double, 6, 6>::Zero();
86-
K.block<3, 3>(0, 0) =
87-
rot_base_impedance * \
88-
vic_command_data.stiffness.block<3, 3>(0, 0) * \
89-
rot_base_impedance.transpose();
90-
K.block<3, 3>(3, 3) =
91-
rot_base_impedance * \
92-
vic_command_data.stiffness.block<3, 3>(3, 3) * \
93-
rot_base_impedance.transpose();
94-
95101
Eigen::Matrix<double, 6, 6> D = Eigen::Matrix<double, 6, 6>::Zero();
96-
D.block<3, 3>(0, 0) =
97-
rot_base_impedance * \
98-
vic_command_data.damping.block<3, 3>(0, 0) * \
99-
rot_base_impedance.transpose();
100-
D.block<3, 3>(3, 3) =
101-
rot_base_impedance * \
102-
vic_command_data.damping.block<3, 3>(3, 3) * \
103-
rot_base_impedance.transpose();
104-
105-
Eigen::Matrix<double, 6, 6> M = Eigen::Matrix<double, 6, 6>::Zero();
106-
M.block<3, 3>(0, 0) =
107-
rot_base_impedance * \
108-
vic_command_data.inertia.block<3, 3>(0, 0) * \
109-
rot_base_impedance.transpose();
110-
M.block<3, 3>(3, 3) =
111-
rot_base_impedance * \
112-
vic_command_data.inertia.block<3, 3>(3, 3) * \
113-
rot_base_impedance.transpose();
102+
registration_MKD(vic_command_data.inertia, M);
103+
registration_MKD(vic_command_data.stiffness, K);
104+
registration_MKD(vic_command_data.damping, D);
114105

115106
// Compute pose tracking errors
116107
Eigen::Matrix<double, 6, 1> error_pose;
@@ -167,22 +158,36 @@ bool VanillaCartesianImpedanceRule::compute_controls(
167158
vic_input_data.end_effector_frame,
168159
J_dot_
169160
);
161+
170162
RCLCPP_DEBUG(logger_, "Computing J_pinv...");
171163
const Eigen::JacobiSVD<Eigen::MatrixXd> J_svd =
172164
Eigen::JacobiSVD<Eigen::MatrixXd>(J_, Eigen::ComputeThinU | Eigen::ComputeThinV);
173165
double conditioning_J = 1000.0;
174166
if (J_.cols() < 6) {
167+
RCLCPP_WARN_THROTTLE(
168+
logger_, internal_clock_, 5000, "Jacobian has only %u columns, expecting at least 6!!!",
169+
J_.cols());
175170
conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(J_.cols() - 1);
176171
} else {
177172
conditioning_J = J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1);
178173
}
179-
if (conditioning_J > 30) {
174+
if (conditioning_J > 40) {
180175
success = false;
176+
std::cerr << "J_svd.singularValues() = " << J_svd.singularValues().transpose() << std::endl;
181177
RCLCPP_ERROR(
182178
logger_,
183179
"Jacobian singularity detected (max(singular values)/min(singular values) = %lf)!",
184180
conditioning_J
185181
);
182+
} else if (conditioning_J > 15) {
183+
RCLCPP_WARN_THROTTLE(
184+
logger_,
185+
internal_clock_,
186+
5000,
187+
"Nearing Jacobian singularity (max(singular values)/min(singular values) = %lf), "
188+
"proceed with caution!",
189+
conditioning_J
190+
);
186191
}
187192
J_pinv_ = (J_.transpose() * J_ + alpha_pinv_ * I_joint_space_).inverse() * J_.transpose();
188193

0 commit comments

Comments
 (0)