Edit task inertia matrix func

common/include/math/robotics_utilities.hpp

@@ -108,8 +108,8 @@ auto getFilteredTustinDerivative(const Eigen::Matrix<T, Rows, 1> & u,      //
 template <typename T, int Rows, int Cols>
 auto getTaskAcceleration(const Eigen::Matrix<T, Rows, Cols> & J,
                          const Eigen::Matrix<T, Rows, Cols> & dJ,
-                         const Eigen::Matrix<T, Rows, 1> & dq,
-                         const Eigen::Matrix<T, Rows, 1> & ddq)
+                         const Eigen::Matrix<T, Cols, 1> & dq,
+                         const Eigen::Matrix<T, Cols, 1> & ddq)
 {
   return J * ddq + dJ * dq;
 };
@@ -133,14 +133,20 @@ auto computeFullInertiaMatrix(const Eigen::Matrix<T, Rows, Cols> & M_ut)
  * @param Jt_inv : (pseudo-)inverse of Jacobian.transpose matrix
  * @return n x n operational inertia matrix
  */
-template <typename T, int Rows, int Cols>
-auto computeTaskInertiaMatrix(const Eigen::Matrix<T, Rows, Cols> & M,
-                              const Eigen::Matrix<T, Rows, Cols> & J_inv,
-                              const Eigen::Matrix<T, Rows, Cols> & Jt_inv)
+template <typename DerivedM, typename DerivedJInv, typename DerivedJtInv>
+auto computeTaskInertiaMatrix(const Eigen::MatrixBase<DerivedM> & M,
+                              const Eigen::MatrixBase<DerivedJInv> & J_inv,
+                              const Eigen::MatrixBase<DerivedJtInv> & Jt_inv)
 {
-  Eigen::Matrix<T, Rows, Cols> Mt(M.rows(), M.cols());
-  Mt.noalias() = Jt_inv * M * J_inv;  // prevent unnecessary duplicate to minimize memory allocation
-  return Mt;
+  using Scalar = typename DerivedM::Scalar;
+
+  // 반환 행렬의 크기를 자동으로 결정
+  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Mt(Jt_inv.rows(), J_inv.cols());
+
+  // 병렬 연산 및 중간 메모리 복사 방지
+  Mt.noalias() = Jt_inv * M * J_inv;
+
+  return Mt.eval();  // 완전히 평가된 행렬 반환
 }
 
 }  // namespace robot_utils

fr3_impedance_controller_sim/src/fr3_impedance_controller_sim.cpp

@@ -350,7 +350,7 @@ void FR3ImpedanceControllerSim::update()
 
     /* compute current EE Twist & Acceleration using backward Euler method */
     ee_vel_world_cal_ = Je * dq_mes_;
-    ee_accel_world_eul_ = robo::getEulerDerivative(ee_vel_world_cal_, ee_vel_world_cal_prev_, dt_);
+    ee_acc_world_eul_ = robo::getEulerDerivative(ee_vel_world_cal_, ee_vel_world_cal_prev_, dt_);
 
     /* Inertia matrix */
     pin::crba(model_, data_, q_mes_);
@@ -359,6 +359,7 @@ void FR3ImpedanceControllerSim::update()
     // k_mass << 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, 10.0;
     // M.diagonal().array() *= k_mass.array();
     Mat7<double> M_full = robo::computeFullInertiaMatrix(M);
+    // TODO: Check Me is correct
     Mat6<double> Me = robo::computeTaskInertiaMatrix(M_full, Je_pinv, Je_t_pinv);
     Mat7<double> dMinv = autodiff_->get_dMinv(q_mes_, dq_mes_);
 
@@ -438,11 +439,10 @@ void FR3ImpedanceControllerSim::update()
     Vec7<double> tau_dist_hat_dob;
     // Vec7<double> tau_dist_hat_dob = Je.transpose() * F_dist_hat_dob;
 
-    // Vec6<double> fr_F_task = Je_t_pinv * fr_tau_des_;
-    // Vec6<double> fr_F_ext_hat_filtered = Je_t_pinv * fr_tau_ext_hat_filtered_;
+    Vec6<double> fr_F_task = Je_t_pinv * fr_tau_des_;
+    Vec6<double> fr_F_ext_hat_filtered = Je_t_pinv * fr_tau_ext_hat_filtered_;
     // !!! since dob has shared pointer, multiple use of this function induce bug in estimation
-    // Vec6<double> fr_F_dist_hat_dob = dob_->get_force_disturbance(fr_F_task, Me,
-    // ee_acc_world_eul_);
+    Vec6<double> fr_F_dist_hat_dob = dob_->get_force_disturbance(fr_F_task, Me, ee_acc_world_eul_);
 
     /* update states */
     this->update_previous_states();