Skip to content

Commit 17a109f

Browse files
Hydran00Hydran00
committed
clik fix
1 parent d754599 commit 17a109f

File tree

3 files changed

+196
-79
lines changed

3 files changed

+196
-79
lines changed

‎effort_controller_base/include/effort_controller_base/effort_controller_base.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -180,6 +180,8 @@ class EffortControllerBase : public controller_interface::ControllerInterface {
180180
std::shared_ptr<KDL::TreeFkSolverPos_recursive> m_forward_kinematics_solver;
181181
std::shared_ptr<KDL::ChainFkSolverPos_recursive> m_fk_solver;
182182
std::shared_ptr<KDL::ChainIkSolverPos_NR_JL> m_ik_solver;
183+
KDL::JntArray m_q_ns;
184+
KDL::JntArray m_weights;
183185

184186
std::shared_ptr<KDL::ChainIkSolverVel_pinv_nso> m_ik_solver_vel_nso;
185187

‎effort_controller_base/src/effort_controller_base.cpp‎

Lines changed: 26 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -188,6 +188,10 @@ EffortControllerBase::on_configure(
188188
// Initialize effort limits
189189
m_joint_effort_limits.resize(m_joint_number);
190190

191+
// initialize postural task
192+
m_q_ns.resize(m_joint_number);
193+
m_weights.resize(m_joint_number);
194+
191195
// Parse joint limits
192196
m_upper_pos_limits.resize(m_joint_number);
193197
m_lower_pos_limits.resize(m_joint_number);
@@ -231,34 +235,35 @@ EffortControllerBase::on_configure(
231235
m_forward_kinematics_solver.reset(new KDL::TreeFkSolverPos_recursive(tmp));
232236
m_fk_solver.reset(new KDL::ChainFkSolverPos_recursive(m_robot_chain));
233237

234-
m_ik_solver_vel.reset(new KDL::ChainIkSolverVel_pinv(m_robot_chain));
238+
m_q_ns(0) = -0.57;
239+
m_q_ns(1) = 0.38;
240+
m_q_ns(2) = 0.12;
241+
m_q_ns(3) = -1.5;
242+
m_q_ns(4) = -0.26;
243+
m_q_ns(5) = 0.89;
244+
m_q_ns(6) = 0.0;
245+
246+
m_weights(0) = 1.0;
247+
m_weights(1) = 1.0;
248+
m_weights(2) = 1.0;
249+
m_weights(3) = 1.0;
250+
m_weights(4) = 1.0;
251+
m_weights(5) = 1.0;
252+
m_weights(6) = 1.0;
253+
254+
m_ik_solver_vel_nso.reset(
255+
new KDL::ChainIkSolverVel_pinv_nso(m_robot_chain, m_q_ns, m_weights));
256+
257+
m_ik_solver_vel.reset(new KDL::ChainIkSolverVel_pinv(m_robot_chain));
235258

236259
m_ik_solver.reset(new KDL::ChainIkSolverPos_NR_JL(
237260
m_robot_chain, m_lower_pos_limits, m_upper_pos_limits, *m_fk_solver,
238-
*m_ik_solver_vel, 100, 1e-6));
261+
*m_ik_solver_vel_nso, 1000, 1e-6));
239262

240263
// m_ik_solver.reset(new KDL::ChainIkSolverPos_LMA(m_robot_chain, 1e-4, 1000,
241264
// 1e-6));
242265

243-
KDL::JntArray q_ns(m_joint_number), weights(m_joint_number);
244-
q_ns(0) = 0.0;
245-
q_ns(1) = 0.78;
246-
q_ns(2) = 0.0;
247-
q_ns(3) = -1.57;
248-
q_ns(4) = 0.0;
249-
q_ns(5) = -0.78;
250-
q_ns(6) = 0.0;
251-
252-
weights(0) = 1.0;
253-
weights(1) = 1.0;
254-
weights(2) = 1.0;
255-
weights(3) = 1.0;
256-
weights(4) = 1.0;
257-
weights(5) = 1.0;
258-
weights(6) = 1.0;
259266

260-
m_ik_solver_vel_nso.reset(
261-
new KDL::ChainIkSolverVel_pinv_nso(m_robot_chain, q_ns, weights));
262267

263268
m_jnt_to_jac_solver.reset(new KDL::ChainJntToJacSolver(m_robot_chain));
264269
m_dyn_solver.reset(new KDL::ChainDynParam(m_robot_chain, grav));
@@ -304,6 +309,7 @@ EffortControllerBase::on_configure(
304309
m_old_joint_velocities.data.setZero();
305310
m_simulated_joint_motion.resize(m_joint_number);
306311

312+
307313
RCLCPP_INFO(get_node()->get_logger(), "Finished Base on_configure");
308314
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
309315
CallbackReturn::SUCCESS;
@@ -388,16 +394,10 @@ void EffortControllerBase::writeJointEffortCmds(
388394
if (target_joint_positions(i) + 2 * max_velocity > m_upper_pos_limits(i)) {
389395
// set equal to current
390396
target_joint_positions(i) = m_upper_pos_limits(i) - 2 * max_velocity - eps;
391-
// RCLCPP_INFO(get_node()->get_logger(),
392-
// "Joint %s target is out of limits, setting to %f",
393-
// m_joint_names[i].c_str(), m_joint_positions(i));
394397
} else if (target_joint_positions(i) - 2 * max_velocity <
395398
m_lower_pos_limits(i)) {
396399
// set equal to current
397400
target_joint_positions(i) = m_lower_pos_limits(i) + 2 * max_velocity + eps;
398-
// RCLCPP_INFO(get_node()->get_logger(),
399-
// "Joint %s target is out of limits, setting to %f",
400-
// m_joint_names[i].c_str(), m_joint_positions(i));
401401
}
402402

403403
m_joint_cmd_pos_handles[i].get().set_value(target_joint_positions(i));

‎kuka_cartesian_impedance_controller/src/kuka_cartesian_impedance_controller.cpp‎

Lines changed: 168 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
1+
#include <fstream>
12
#include <kuka_cartesian_impedance_controller/kuka_cartesian_impedance_controller.h>
2-
33
namespace kuka_cartesian_impedance_controller {
44

55
KukaCartesianImpedanceController::KukaCartesianImpedanceController()
@@ -37,6 +37,95 @@ KukaCartesianImpedanceController::on_configure(
3737
CallbackReturn::SUCCESS) {
3838
return ret;
3939
}
40+
// Open the CSV file *before* the loop
41+
// std::ofstream file("cartesian_transforms.csv");
42+
// std::vector<Eigen::Matrix4d> cartesian_coordinates;
43+
// KDL::JntArray q(Base::m_joint_number);
44+
// KDL::Frame frame;
45+
// Eigen::Matrix4d m_transform = Eigen::Matrix4d::Identity();
46+
// Eigen::Matrix3d m_rotation = Eigen::Matrix3d::Identity();
47+
// const double increment = 0.2;
48+
// int confs = 0;
49+
// Eigen::Vector3d YPR;
50+
// auto compare_angles = [](double angle1, double angle2, double tolerance) {
51+
// auto normalize_angle = [](double angle) {
52+
// angle = std::fmod(angle + M_PI, 2.0 * M_PI);
53+
// if (angle < 0) {
54+
// angle += 2.0 * M_PI;
55+
// }
56+
// return angle - M_PI;
57+
// };
58+
// return std::abs(normalize_angle(angle1) - normalize_angle(angle2)) <
59+
// tolerance;
60+
// };
61+
// for (double A1 = Base::m_lower_pos_limits(0);
62+
// A1 < Base::m_upper_pos_limits(0); A1 += increment) {
63+
64+
// for (double A2 = Base::m_lower_pos_limits(1);
65+
// A2 < Base::m_upper_pos_limits(1); A2 += increment) {
66+
67+
// for (double A3 = Base::m_lower_pos_limits(2);
68+
// A3 < Base::m_upper_pos_limits(2); A3 += increment) {
69+
70+
// // for (double A4 = Base::m_lower_pos_limits(3);
71+
// // A4 < Base::m_upper_pos_limits(3); A4 += increment) {
72+
73+
// for (double A5 = Base::m_lower_pos_limits(4);
74+
// A5 < Base::m_upper_pos_limits(4); A5 += increment) {
75+
76+
// for (double A6 = Base::m_lower_pos_limits(5);
77+
// A6 < Base::m_upper_pos_limits(5); A6 += increment) {
78+
// q.data(0) = A1;
79+
// q.data(1) = A2;
80+
// q.data(2) = A3;
81+
// q.data(3) = Base::m_lower_pos_limits(3);
82+
// q.data(4) = A5;
83+
// q.data(5) = A6;
84+
// q.data(6) = 0.0;
85+
86+
// Base::m_fk_solver->JntToCart(q, frame);
87+
// m_rotation = Eigen::Map<const Eigen::Matrix3d>(frame.M.data);
88+
// // Convert rotation matrix to yaw-pitch-roll angles
89+
// YPR = m_rotation.eulerAngles(2, 1, 0);
90+
// // 3.09297, 0.0526055, 2.71678
91+
// if (compare_angles(YPR[1], 0.0526055, 0.01) &&
92+
// compare_angles(YPR[2], 2.71678, 0.01)) {
93+
94+
// m_transform.block<3, 3>(0, 0) =
95+
// Eigen::Map<const Eigen::Matrix3d>(frame.M.data);
96+
// m_transform.block<3, 1>(0, 3) =
97+
// Eigen::Map<const Eigen::Vector3d>(frame.p.data);
98+
99+
// // Write directly to file
100+
// for (int i = 0; i < 4; ++i) {
101+
// for (int j = 0; j < 4; ++j) {
102+
// file << m_transform(i, j);
103+
// if (i != 3 || j != 3)
104+
// file << ",";
105+
// }
106+
// }
107+
// file << "\n";
108+
// }
109+
// confs++;
110+
// if (confs % 100000 == 0) {
111+
// RCLCPP_INFO(get_node()->get_logger(),
112+
// "Computed and saved %d configurations", confs);
113+
// }
114+
// }
115+
// // }
116+
// }
117+
// }
118+
// }
119+
// }
120+
121+
// // Close the file after all transforms are written
122+
// file.close();
123+
// RCLCPP_INFO(
124+
// get_node()->get_logger(),
125+
// "Successfully dumped %d configurations to cartesian_transforms.csv",
126+
// confs);
127+
128+
// log file
40129

41130
m_target_frame_subscriber =
42131
get_node()->create_subscription<geometry_msgs::msg::PoseStamped>(
@@ -75,6 +164,7 @@ KukaCartesianImpedanceController::on_configure(
75164
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
76165
CallbackReturn::SUCCESS;
77166
}
167+
78168
#if LOGGING
79169
void KukaCartesianImpedanceController::stateCallback(
80170
const lbr_fri_idl::msg::LBRState::SharedPtr state) {
@@ -103,8 +193,17 @@ KukaCartesianImpedanceController::on_activate(
103193
m_target_joint_position = Base::m_joint_positions.data;
104194

105195
m_q_starting_pose = Base::m_joint_positions.data;
196+
RCLCPP_INFO_STREAM(
197+
get_node()->get_logger(),
198+
"Starting configuration: " << m_q_starting_pose.transpose());
199+
double roll, pitch, yaw;
200+
m_current_frame.M.GetRPY(roll, pitch, yaw);
106201
RCLCPP_INFO_STREAM(get_node()->get_logger(),
107-
"Starting pose: " << m_q_starting_pose.transpose());
202+
"Current frame: " << m_current_frame.p.x() << ", "
203+
<< m_current_frame.p.y() << ", "
204+
<< m_current_frame.p.z() << ", "
205+
<< roll << ", " << pitch << ", "
206+
<< yaw);
108207
// m_q_starting_pose[0] = 0.0;
109208
// m_q_starting_pose[1] = 0.0;
110209
// m_q_starting_pose[2] = 0.0;
@@ -335,8 +434,10 @@ void KukaCartesianImpedanceController::computeTargetPos() {
335434
Eigen::MatrixXd JJt;
336435
KDL::Jacobian J(Base::m_joint_number);
337436

338-
KDL::JntArray q(Base::m_joint_number);
339-
q.data = Base::m_joint_positions.data;
437+
KDL::JntArray q_clik(Base::m_joint_number);
438+
q_clik.data = Base::m_joint_positions.data;
439+
440+
ctrl::VectorND q_kdl(Base::m_joint_number);
340441

341442
KDL::JntArray dq(Base::m_joint_number);
342443
dq.data.setZero();
@@ -347,87 +448,101 @@ void KukaCartesianImpedanceController::computeTargetPos() {
347448
const double eps = 2e-4;
348449
const int IT_MAX = 50;
349450
const double DT = 0.1;
350-
451+
const double eps_grad_ns = 1e-3;
351452
typedef Eigen::Matrix<double, 6, 1> Vector6d;
352453
Vector6d err, dq_vec;
353-
454+
ctrl::VectorND ns_err(Base::m_joint_number), ns_err_old(Base::m_joint_number);
354455
// Pre-allocate everything outside the loop
355456
ctrl::MatrixND J_pinv(Base::m_joint_number,
356457
6); // assuming shape [n_joints x 6]
357458
KDL::Twist delta_twist;
358459
int i = 0, j = 0;
460+
ns_err.setZero();
461+
ns_err_old.setZero();
462+
359463
for (i = 0;; i++) {
360464
// Compute forward kinematics
361-
Base::m_fk_solver->JntToCart(q, simulated_frame);
465+
Base::m_fk_solver->JntToCart(q_clik, simulated_frame);
466+
ns_err = Base::m_q_ns.data - q_clik.data;
467+
362468
// Compute error
363469
delta_twist = KDL::diff(simulated_frame, m_filtered_frame);
364470
for (j = 0; j < 6; ++j) {
365471
err(j) = delta_twist[j];
366472
}
367473
// Check for convergence
368-
if (err.norm() < eps) {
369-
break;
370-
}
371-
if (i >= IT_MAX) {
372-
// RCLCPP_WARN_STREAM(get_node()->get_logger(),
373-
// "CLIK reached max iterarion -> final_error: " <<
374-
// err.norm());
474+
if ((err.norm() < eps && (ns_err - ns_err_old).norm() < eps_grad_ns) ||
475+
(i > IT_MAX)) {
476+
// RCLCPP_INFO(get_node()->get_logger(),
477+
// "CLIK converged after %d iterations: ns error %f", i,
478+
// ns_err.norm());
375479
break;
376480
}
481+
// RCLCPP_INFO(get_node()->get_logger(), "%d %d %d", err.norm() < eps,
482+
// (ns_err - ns_err_old).norm() < eps_grad_ns, i > IT_MAX);
483+
ns_err_old = ns_err;
377484

378485
// Compute the Jacobian at the current confiation
379-
Base::m_jnt_to_jac_solver->JntToJac(q, J);
486+
Base::m_jnt_to_jac_solver->JntToJac(q_clik, J);
380487
// Compute the damped pseudo-inverse of the Jacobian
381488
pseudoInverse(J.data, &J_pinv);
382489
// 6. Compute dq = J⁺ * err + (I - J⁺J)(q0 - q) / dt
383-
dq.data = J_pinv * err + (m_identity - J_pinv * J.data) *
384-
(m_q_starting_pose - q.data) / DT;
490+
dq.data = J_pinv * err + (m_identity - J_pinv * J.data) * ns_err / DT;
491+
492+
m_logger->add("ns_err", ns_err);
385493

386494
// Integrate joint velocities (Euler integration)
387495
for (j = 0; j < Base::m_joint_number; j++) {
388-
q(j) += dq.data(j) * DT;
496+
q_clik(j) += dq.data(j) * DT;
389497
}
390498
}
499+
// Base::computeIKSolution(m_filtered_frame, q_kdl);
500+
501+
// KDL::Jacobian J;
502+
// J.resize(Base::m_joint_number);
503+
// Base::m_jnt_to_jac_solver->JntToJac(q, J);
391504

392505
const double alpha = 0.01;
393-
auto filtered_q = alpha * q.data + (1 - alpha) * m_target_joint_position;
506+
auto filtered_q = alpha * q_clik.data + (1 - alpha) * m_target_joint_position;
394507
m_target_joint_position = filtered_q;
508+
// m_target_joint_position = m_q_starting_pose;
395509

396510
#if LOGGING
397-
if (m_received_target_frame) {
398-
m_logger->add("time_sec", get_node()->get_clock()->now().seconds());
399-
m_logger->add("time_nsec", get_node()->get_clock()->now().nanoseconds());
400-
// add cartesian traj
401-
m_logger->add("cart_target_x", m_target_frame.p.x());
402-
m_logger->add("cart_target_y", m_target_frame.p.y());
403-
m_logger->add("cart_target_z", m_target_frame.p.z());
404-
Eigen::Matrix<double, 3, 3> rot_des(m_target_frame.M.data);
405-
m_logger->add("cart_target_M", rot_des);
406-
m_logger->add("cart_measured_x", m_current_frame.p.x());
407-
m_logger->add("cart_measured_y", m_current_frame.p.y());
408-
m_logger->add("cart_measured_z", m_current_frame.p.z());
409-
Eigen::Matrix<double, 3, 3> M_c(m_current_frame.M.data);
410-
m_logger->add("cart_measured_M", M_c);
411-
// solver outcome
412-
m_logger->add("joint_measured", Base::m_joint_positions.data);
413-
m_logger->add("joint_target_IK", q.data);
414-
m_logger->add("joint_target_filtered", m_target_joint_position);
415-
416-
std::vector<double> measured_torque(std::begin(m_state.measured_torque),
417-
std::end(m_state.measured_torque));
418-
m_logger->add("measured_torque", measured_torque);
419-
std::vector<double> commanded_torque(std::begin(m_state.commanded_torque),
420-
std::end(m_state.commanded_torque));
421-
m_logger->add("commanded_torque", commanded_torque);
422-
423-
auto compute_condition_number = [](const Eigen::MatrixXd &matrix) {
424-
Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix);
425-
Eigen::VectorXd singular_values = svd.singularValues();
426-
return singular_values(0) / singular_values(singular_values.size() - 1);
427-
};
428-
m_logger->add("condition_number_jac", compute_condition_number(J.data));
429-
// m_logger->flush_available_data();
430-
}
511+
// if (m_received_target_frame) {
512+
m_logger->add("time_sec", get_node()->get_clock()->now().seconds());
513+
m_logger->add("time_nsec", get_node()->get_clock()->now().nanoseconds());
514+
// add cartesian traj
515+
m_logger->add("cart_target_x", m_target_frame.p.x());
516+
m_logger->add("cart_target_y", m_target_frame.p.y());
517+
m_logger->add("cart_target_z", m_target_frame.p.z());
518+
Eigen::Matrix<double, 3, 3> rot_des(m_target_frame.M.data);
519+
m_logger->add("cart_target_M", rot_des);
520+
m_logger->add("cart_measured_x", m_current_frame.p.x());
521+
m_logger->add("cart_measured_y", m_current_frame.p.y());
522+
m_logger->add("cart_measured_z", m_current_frame.p.z());
523+
Eigen::Matrix<double, 3, 3> M_c(m_current_frame.M.data);
524+
m_logger->add("cart_measured_M", M_c);
525+
// solver outcome
526+
m_logger->add("joint_measured", Base::m_joint_positions.data);
527+
m_logger->add("joint_target_IK_kdl", q_kdl);
528+
m_logger->add("joint_target_filtered_kdl", m_target_joint_position);
529+
m_logger->add("joint_target_clik", q_clik.data);
530+
531+
std::vector<double> measured_torque(std::begin(m_state.measured_torque),
532+
std::end(m_state.measured_torque));
533+
m_logger->add("measured_torque", measured_torque);
534+
std::vector<double> commanded_torque(std::begin(m_state.commanded_torque),
535+
std::end(m_state.commanded_torque));
536+
m_logger->add("commanded_torque", commanded_torque);
537+
538+
auto compute_condition_number = [](const Eigen::MatrixXd &matrix) {
539+
Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix);
540+
Eigen::VectorXd singular_values = svd.singularValues();
541+
return singular_values(0) / singular_values(singular_values.size() - 1);
542+
};
543+
// m_logger->add("condition_number_jac", compute_condition_number(J.data));
544+
// m_logger->flush_available_data();
545+
// }
431546
#endif
432547
}
433548

0 commit comments

Comments
 (0)