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/include/franka_example_controllers/cartesian_impedance_example_controller.h

+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+/* ROS packages */
+#include <controller_interface/multi_interface_controller.h>
+#include <dynamic_reconfigure/server.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <geometry_msgs/PoseStamped.h>        // interactive marker
+#include <geometry_msgs/Twist.h>              // spacenave teleop
+#include <sensor_msgs/Joy.h>                  // joystick
+#include "fr3_custom_msgs/ControllerState.h"  // custom message
+
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
+
+// #include <franka_example_controllers/compliance_paramConfig.h>
+#include "franka_example_controllers/controller_paramConfig.h"
+
+/* Dependencies */
+#include "pinocchio/algorithm/crba.hpp"        // joint-space inertia (upper-triangle)
+#include "pinocchio/algorithm/frames.hpp"      //
+#include "pinocchio/algorithm/jacobian.hpp"    //
+#include "pinocchio/algorithm/kinematics.hpp"  //
+#include "pinocchio/algorithm/rnea.hpp"        // Coriolis, gravity, joint torque
+#include "pinocchio/autodiff/casadi.hpp"       //
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+
+/* Custom libraries */
+#include "DisturbanceObserver.hpp"
+#include "ImpedanceController.hpp"
+#include "MomentumObserver.hpp"
+#include "SlidingModeObserver.hpp"
+
+#include "eigen_types.hpp"
+#include "Timer.hpp"
+
+namespace franka_example_controllers
+{
+class CartesianImpedanceExampleController :
+  public controller_interface::MultiInterfaceController<franka_hw::FrankaModelInterface,
+                                                        hardware_interface::EffortJointInterface,
+                                                        franka_hw::FrankaStateInterface>
+{
+public:
+  bool init(hardware_interface::RobotHW * robot_hw, ros::NodeHandle & node_handle) override;
+  void starting(const ros::Time &) override;
+  void update(const ros::Time &, const ros::Duration & period) override;
+  // void stopping(const ros::Time &) override;
+
+private:
+  std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
+  std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
+  std::vector<hardware_interface::JointHandle> joint_handles_;
+
+  Timer loop_timer_;
+  double max_exec_time_ = 0.0;
+  // int tick_ = 0;            // loop iteration
+  double loop_time_ = 0.0;  // loop elapsed time
+
+  /* Robot Model */
+  // NOTE: you need to specify absolute path (not relative) for URDF file.
+  std::string repo_path_ = "/home/ipk410/nix-project/impedance_controller";
+  std::string urdf_filename_ = std::string(repo_path_ + "/assets/model/fr3_franka_hand.urdf");
+  pinocchio::Model model_;
+  pinocchio::Data data_;
+  int ee_id_;
+
+  //* ----- Constants ------------------------------------------------------------------------------
+  const double delta_tau_max_{ 1.0 };
+  const double twist_scale_[6] = { 0.15, 0.15, 0.15, 0.8, 0.8, 0.8 };
+  const double alpha_ = 0.005;  // targeting filter coefficient
+  // const double spacenav_cmd_max_{ 0.6834 };
+
+  // TODO: make it as rosparam (optional)
+  const Vec6<double> fr3_vel_limit_ =  // [m(rad)/s]
+    (Vec6<double>() << 3.0, 3.0, 3.0, 2.5, 2.5, 2.5).finished();
+  const Vec6<double> fr3_acc_limit_ =  // [m(rad)/s^2]
+    (Vec6<double>() << 9.0, 9.0, 9.0, 17.0, 17.0, 17.0).finished();
+  const Vec6<double> fr3_jrk_limit_ =  // [m(rad)/s^3]
+    (Vec6<double>() << 4500.0, 4500.0, 4500.0, 8500.0, 8500.0, 8500.0).finished();
+
+  //* ----- Booleans to control --------------------------------------------------------------------
+  bool b_count_loop_time_ = false;
+
+  bool b_dob_applied_ = false;
+  bool b_sosml_applied_ = false;
+  bool b_mob_applied_ = false;
+
+  //* ----- Publisher & Subscriber -----------------------------------------------------------------
+  /* Controller state publisher */
+  ros::Publisher pub_ctrl_state_;
+  fr3_custom_msgs::ControllerState msg_ctrl_state_;
+
+  /* Target EE Twist & Gripper command from Spacenav (or PS3) Joystick */
+  ros::Subscriber sub_target_twist_;
+  ros::Subscriber sub_joy_;
+  // std::array<int, 2> btn_cmd_;
+  Vec6<double> ee_vel_world_target_;
+
+  /* Target EE Pose command from Interactive Marker */
+  // ros::Subscriber sub_ee_pose_;
+  // std::mutex target_ee_pose_mutex_;
+  // Vec3<double> ee_pos_world_target_;
+  // Quat<double> ee_quat_world_target_;
+  // bool b_is_marker_mode_ = false;
+
+  //* ----- Control Parameters ---------------------------------------------------------------------
+  /* Dynamic Reconfigure Server */
+  std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::controller_paramConfig>>
+    ctrl_param_server_;
+  ros::NodeHandle ctrl_param_node_;
+
+  Mat6<double> K_task_, K_task_target_;  // cartesian stiffness [N/m, Nm/rad]
+  Mat6<double> D_task_, D_task_target_;  // cartesian damping [Ns/m, Nms/rad]
+  Mat7<double> K_null_, K_null_target_;  // null-sapce stiffness [Nm/rad]
+  Mat7<double> D_null_, D_null_target_;  // null-space damping [Nms/rad]
+
+  Vec6<double> fq_, fq_target_;                   // DOB low-pass filter cutoff-frequency [Hz]
+  Vec7<double> s1_, s1_target_, s2_, s2_target_;  // SOSML tuning parameters [Hz^2]
+  Vec7<double> t1_, t1_target_, t2_, t2_target_;  // SOSML tuning parameters [Hz]
+  Vec7<double> r_th_, r_th_target_;               // MOB residual threshold [Nm]
+  Vec7<double> km_, km_target_;                   // MOB low-pass filter gain [rad/s]
+
+  //* ----- Control States -------------------------------------------------------------------------
+  /* Joint states */
+  Vec7<double> q_mes_;
+  Vec7<double> q_null_des_;
+  Vec7<double> dq_mes_, dq_mes_prev_;
+  Vec7<double> ddq_eul_, ddq_eul_prev_;  // joint accel using backward Euler method
+
+  /* Desired EE states */
+  Vec3<double> ee_pos_world_des_;
+  Quat<double> ee_quat_world_des_;
+  Vec3<double> ee_rpy_world_des_, ee_rpy_world_des_prev_;
+  Vec6<double> ee_vel_world_des_, ee_vel_world_des_prev_;
+  Vec6<double> ee_acc_world_des_, ee_acc_world_des_prev_;
+
+  /* Measured EE states */
+  Vec3<double> ee_pos_world_mes_;
+  Quat<double> ee_quat_world_mes_;
+  Vec3<double> ee_rpy_world_mes_, ee_rpy_world_mes_prev_;
+  Vec6<double> ee_vel_world_mes_, ee_vel_world_mes_prev_;  // v = J * dq
+  Vec6<double> ee_acc_world_eul_, ee_acc_world_eul_prev_;  // task accel by backward Euler method
+
+  /* Controllers */
+  std::unique_ptr<ImpedanceController<double>> imp_;
+  std::unique_ptr<DisturbanceObserver<double>> dob_;
+  Vec7<double> tau_d_hat_dob_;
+  Vec6<double> Fd_hat_dob_;
+
+  std::unique_ptr<SlidingModeObserver<double>> sosml_;
+  Vec7<double> ddq_hat_sosml_;
+  Vec7<double> tau_d_hat_sosml_;
+  Vec6<double> ee_acc_world_hat_sosml_;
+  Vec6<double> Fd_hat_sosml_;
+
+  std::unique_ptr<MomentumObserver<double>> mob_;
+  Vec7<double> h_mes_, h_mes_prev_;  // General Momentum
+  Vec7<double> ddq_hat_mob_;
+  Vec6<double> ee_acc_world_hat_mob_;
+  Vec7<double> tau_d_hat_mob_;
+  Vec6<double> Fd_hat_mob_;
+
+  Vec6<double> Fd_hat_fr_;  // franka's external force estimation
+
+public:
+  //* ----- SUBSCRIBE CALLBACK ---------------------------------------------------------------------
+  void subscribe_target_twist(const geometry_msgs::TwistConstPtr & msg);
+  void subscribe_joy_command(const sensor_msgs::JoyConstPtr & msg);
+
+  //* ----- PARAMETER SERVER CALLBACK --------------------------------------------------------------
+  void request_controller_parameters(franka_example_controllers::controller_paramConfig & config,
+                                     uint32_t level);
+
+  //* ----- METHODS --------------------------------------------------------------------------------
+  void init_states();
+  void update_previous_states();
+  void publish_controller_state();
+
+  /* joint torque saturation */
+  Eigen::Matrix<double, 7, 1> saturateTorqueRate(
+    const Eigen::Matrix<double, 7, 1> & tau_d_calculated,
+    const Eigen::Matrix<double, 7, 1> & tau_J_d);  // NOLINT (readability-identifier-naming)
+
+  //* ----- PRINTER --------------------------------------------------------------------------------
+  void print_end_effector_states();
+  void print_dynamics();
+};
+
+}  // namespace franka_example_controllers