Package fr3_impedance_controller_real in nix env

Including all deps. Tested with nix develop then roslaunch franka_example_controllers ....

Issue with libfranka version, in the end needed to use fetchGithub in Nix to get proper files.

Closes #7 (closed)

fr3_impedance_controller_real/src/cartesian_impedance_example_controller.cpp

@@ -188,19 +188,19 @@ void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
   ee_pos_world_des_ += v_des * period.toSec();
   ee_quat_world_des_ = ori::getNewQuat(ee_quat_world_des_, w_des, period.toSec());
   ee_rpy_world_des_ = ori::quaternionToRPY(ee_quat_world_des_, ee_rpy_world_des_prev_);
+  // std::cout << "Desired EE pos [m]:\t" << ee_pos_world_des_.transpose() << std::endl;
+  // std::cout << "Desired EE RPY [rad]:\t" << ee_rpy_world_des_.transpose() << std::endl;
 
   //* ----- get Current States  --------------------------------------------------------------------
   franka::RobotState robot_state = state_handle_->getRobotState();
 
   Eigen::Map<Vec7<double>> q(robot_state.q.data());
   Eigen::Map<Vec7<double>> dq(robot_state.dq.data());
+  q_mes_ = Eigen::Map<const Vec7<double>>(robot_state.q.data());
   dq_mes_ = Eigen::Map<const Vec7<double>>(robot_state.dq.data());
-  cout << "q:\t" << q.transpose() << endl;
-  cout << "dq:\t" << dq.transpose() << endl;
-  cout << "dq_mes:\t" << dq_mes_.transpose() << endl;
 
-  Eigen::Map<Vec7<double>> tau_J_d(  // NOLINT (readability-identifier-naming)
-    robot_state.tau_J_d.data());
+  // NOLINT (readability-identifier-naming)
+  Eigen::Map<Vec7<double>> tau_J_d(robot_state.tau_J_d.data());
 
   ddq_eul_ = robo::getEulerDerivative(dq_mes_, dq_mes_prev_, dt_);
 
@@ -210,12 +210,14 @@ void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
 
   ee_pos_world_mes_ = data_.oMf[ee_id_].translation();
   RotMat<double> R_OE = data_.oMf[ee_id_].rotation();
+
   ee_quat_world_mes_ = Eigen::Quaterniond(R_OE);
   ee_rpy_world_mes_ = ori::quaternionToRPY(orientation, ee_rpy_world_mes_prev_);
-  // std::cout << "Desired EE pos [m]:\t" << ee_pos_world_des_.transpose() << std::endl;
-  // std::cout << "Measured EE pos [m]:\t" << position.transpose() << std::endl;
-  // std::cout << "Desired EE RPY [rad]:\t" << ee_rpy_world_des_.transpose() << std::endl;
-  // std::cout << "Measured EE RPY [rad]:\t" << ee_rpy_world_mes_.transpose() << std::endl;
+  // NOTE: results from FR3 state & pinocchio are almost the same.
+  // cout << "Measured Pin EE pos [m]:\t" << ee_pos_world_mes_.transpose() << endl;
+  // cout << "Measured FR3 EE pos [m]:\t" << position.transpose() << endl;
+  // cout << "Measured Pin EE quat:\t" << ee_quat_world_mes_ << endl;
+  // cout << "Measured FR3 EE quat:\t" << orientation << endl;
 
   //* ----- compute Kinematics & Dynamics ----------------------------------------------------------
   // NOTE: FR3 model states
@@ -240,6 +242,11 @@ void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
   Mat76<double> Je_t = Je.transpose();
   MatX<double> Je_pinv = Je.completeOrthogonalDecomposition().pseudoInverse();
   MatX<double> Je_t_pinv = Je.transpose().completeOrthogonalDecomposition().pseudoInverse();
+  // cout << "FR3 Jacb:\n" << jacobian << endl;
+  // cout << "Pin Jacb:\n" << Je << endl;
+  // DEBUG: Jacoian pinv results from FR3 & pinocchio are different
+  // cout << "FR3 Jacb pinv:\n" << jacobian_transpose_pinv << endl;
+  // cout << "Pin Jacb pinv:\n" << Je_t_pinv << endl;
 
   /* compute current EE Twist & Acceleration using backward Euler method */
   ee_vel_world_cal_ = Je * dq_mes_;
@@ -260,8 +267,8 @@ void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
   pin::computeCoriolisMatrix(model_, data_, q_mes_, dq_mes_);
   Mat7<double> C = data_.C;
   Vec7<double> tau_c = C * dq_mes_;
-  // cout << "FR3:\t" << coriolis.transpose() << endl;
-  // cout << "Pin:\t" << tau_c.transpose() << endl;
+  // cout << "Coriolis FR3:\t" << coriolis.transpose() << endl;
+  // cout << "Coriolis Pin:\t" << tau_c.transpose() << endl;
 
   //* ----- CONTROL LAW ----------------------------------------------------------------------------
   /* Cartesian impedance control */
@@ -281,24 +288,24 @@ void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
   Vec6<double> ee_pose_err_prev = imp_->get_previous_pose_error();
   Vec6<double> dpose_err = robo::getEulerDerivative(ee_pose_err, ee_pose_err_prev, dt_);
   Vec6<double> F_task = imp_->get_task_impedance_force(dpose_err);
-  // cout << "FR3:\t" << tau_task.transpose() << endl;
-  // cout << "My:\t" << (Je_t * F_task).transpose() << endl;
+  // cout << "Imp FR3:\t" << tau_task.transpose() << endl;
+  // cout << "Imp My:\t" << (Je_t * F_task).transpose() << endl;
 
   /* Task-Force Disturbance compensation */
   // Vec7<double> tau_task = Je_t * (F_task - (b_dob_applied_ ? Fd_hat_dob_ :
   // Vec6<double>::Zero()));
 
   /* Null-space impedance control */
-  Vec7<double> tan_null =
+  Vec7<double> tau_null =
     (Eigen::MatrixXd::Identity(7, 7) - jacobian.transpose() * jacobian_transpose_pinv) *
     (K_null_ * (q_null_des_ - q) - D_null_ * dq);
   imp_->compute_null_error(q_null_des_, q);
   Vec7<double> tau_null_temp = imp_->get_null_impedance_torque(dq_mes_, Je, Je_t_pinv);
-  // cout << "FR3:\t" << tau_null.transpose() << endl;
-  // cout << "My:\t" << tau_null_temp.transpose() << endl;
+  // cout << "Null FR3:\t" << tau_null.transpose() << endl;
+  // cout << "Null My:\t" << tau_null_temp.transpose() << endl;
 
   /* compute desired joint torque command */
-  Vec7<double> tau_d = tau_task + tan_null + coriolis;
+  Vec7<double> tau_d = tau_task + tau_null + coriolis;
   tau_d << saturateTorqueRate(tau_d, tau_J_d);  // Saturate torque rate to avoid discontinuities
 
   //* ----- Observers ------------------------------------------------------------------------------
@@ -417,6 +424,7 @@ void CartesianImpedanceExampleController::complianceParamCallback(
 //* ----- METHODS ----------------------------------------------------------------------------------
 void CartesianImpedanceExampleController::init_states()
 {
+  q_mes_.setZero();
   dq_mes_.setZero();
   dq_mes_prev_.setZero();
   ddq_eul_.setZero();