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

@@ -29,6 +29,7 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW * rob
                                                ros::NodeHandle & node_handle)
 {
   //* Load URDF model and data *//
+
   if (urdf_filename_.empty())
   {
     throw std::runtime_error("URDF filename is invalid");
@@ -39,7 +40,7 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW * rob
   // cout << "Number of frames: " << model_.frames.size() << endl;
   // cout << "Number of bodies: " << model_.nv << endl;
 
-  ee_id_ = model_.getFrameId("tcp_link", (pin::FrameType::BODY));  // tcp
+  ee_id_ = model_.getFrameId("fr3_hand_tcp", (pin::FrameType::BODY));  // tcp
 
   //* Set Subscriber *//
   sub_equilibrium_pose_ = node_handle.subscribe(
@@ -172,6 +173,19 @@ void CartesianImpedanceExampleController::starting(const ros::Time & /*time*/)
 
   // set nullspace equilibrium configuration to initial q
   q_d_nullspace_ = q_initial;
+
+  // if (urdf_filename_.empty())
+  // {
+  //   throw std::runtime_error("URDF filename is invalid");
+  // }
+  // pin::urdf::buildModel(urdf_filename_, model_);
+  // data_ = pin::Data(model_);
+  // std::cout << "Number of joints: " << model_.nq << std::endl;
+  // std::cout << "Number of frames: " << model_.frames.size() << std::endl;
+  // std::cout << "Number of bodies: " << model_.nv << std::endl;
+
+  // ee_id_ = model_.getFrameId("fr3_hand_tcp", (pin::FrameType::BODY));  // tcp
+  // std::cout << "End-effector frame ID: " << ee_id_ << std::endl;
 }
 
 void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
@@ -205,8 +219,11 @@ void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
   Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
   Eigen::Vector3d position(transform.translation());
   Eigen::Quaterniond orientation(transform.rotation());
-  ee_rpy_world_mes_ = ori::quaternionToRPY(orientation, ee_rpy_world_mes_prev_);
-  // this->print_end_effector_states();
+  ee_rpy_world_mes_ = orient_utils::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;
 
   // compute error to desired pose
   Eigen::Matrix<double, 6, 1> error;