Skip to content
Snippets Groups Projects

Package fr3_impedance_controller_real in nix env

Merged Kevin Haninger requested to merge 7-add-local-package-to-nix-flake-new into main
2 files
+ 44
93
Compare changes
  • Side-by-side
  • Inline
Files
2
// 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
Loading