Skip to content
Snippets Groups Projects

Package fr3_impedance_controller_real in nix env

Merged Package fr3_impedance_controller_real in nix env
7-add-local-package-to-nix-flake-new into main
1 unresolved thread
Merged Kevin Haninger requested to merge 7-add-local-package-to-nix-flake-new into main
1 unresolved thread
Compare
and
Show latest version
3 files
+ 264
− 194
Compare changes
  • Side-by-side
  • Inline
Files
3
fr3_impedance_controller_real/include/franka_example_controllers/cartesian_impedance_example_controller.h
+ 133
− 63
@@ -7,19 +7,17 @@
#include <string>
#include <vector>
/* ROS packages */
#include <controller_interface/multi_interface_controller.h>
#include <dynamic_reconfigure/server.h>
#include <geometry_msgs/PoseStamped.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <ros/node_handle.h>
#include <ros/time.h>
#include <Eigen/Dense>
#include <geometry_msgs/Twist.h> // for spacenave teleop
#include <sensor_msgs/Joy.h> // for joystick command
#include "eigen_types.hpp" // for custom Eigen types
#include "orientation_utilities.hpp" // for orientation computation
#include <geometry_msgs/PoseStamped.h> // for interactive marker
#include <geometry_msgs/Twist.h> // for spacenave teleop
#include <sensor_msgs/Joy.h> // for joystick command
#include <franka_example_controllers/compliance_paramConfig.h>
#include <franka_hw/franka_model_interface.h>
@@ -34,14 +32,17 @@
#include "pinocchio/autodiff/casadi.hpp" //
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/urdf.hpp"
// #include <Eigen/Dense>
/* Controllers */
/* Custom libraries */
#include "DisturbanceObserver.hpp"
#include "ImpedanceController.hpp"
#include "MomentumObserver.hpp"
#include "SlidingModeObserver.hpp"
#include "eigen_types.hpp"
namespace franka_example_controllers
{
class CartesianImpedanceExampleController :
@@ -52,92 +53,161 @@ class CartesianImpedanceExampleController :
public:
bool init(hardware_interface::RobotHW * robot_hw, ros::NodeHandle & node_handle) override;
void starting(const ros::Time &) override;
// void stopping(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_;
double filter_params_{ 0.005 };
double nullspace_stiffness_{ 20.0 };
double nullspace_stiffness_target_{ 20.0 };
int tick_; // loop iteration
double dt_; // loop timestep
double loop_time_ = 0.0; // loop elapsed time
//* ----- Constants ------------------------------------------------------------------------------
const double delta_tau_max_{ 1.0 };
const double spacenav_cmd_max_{ 0.6834 };
const double alpha_ = 0.005; // parameter filter coefficient
// 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_impedance_applied_ = true;
// bool b_dob_applied_ = false;
// bool b_smo_applied_ = false;
// bool b_mob_applied_ = false;
//* ----- Publisher & Subscriber -----------------------------------------------------------------
/* Target EE Twist & Gripper command from Spacenav Joystick */
ros::Subscriber sub_target_twist_;
Vec6<double> ee_vel_scale_{ 0.1, 0.1, 0.1, 0.1, 0.1, 0.1 }; // TODO: make it as rosparam
// Vec6<double> ee_vel_world_target_;
ros::Subscriber sub_joy_;
std::array<int, 2> btn_cmd_;
const std::vector<double> fric_phi1{
0.54615, 0.87224, 0.64068, 1.2794, 0.83904, 0.30301, 0.56489
};
const std::vector<double> fric_phi2{ 5.1181, 9.0657, 10.136, 5.590, 8.3469, 17.133, 10.336 };
// const std::vector<double> fric_phi3{0.039533, 0.025882, -0.04607, 0.036194, 0.026226,
// -0.021047, 0.0035526};
const std::vector<double> fric_phi3{
0., 0., 0., 0., 0., 0., 0.,
};
Eigen::Matrix<double, 6, 6> cartesian_stiffness_;
Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_;
Eigen::Matrix<double, 6, 6> cartesian_damping_;
Eigen::Matrix<double, 6, 6> cartesian_damping_target_;
Eigen::Matrix<double, 7, 1> q_d_nullspace_;
Eigen::Vector3d position_d_;
Eigen::Quaterniond orientation_d_;
/* Target EE Pose command from Interactive Marker */
ros::Subscriber sub_equilibrium_pose_;
std::mutex position_and_orientation_d_target_mutex_;
Eigen::Vector3d position_d_target_;
Eigen::Quaterniond orientation_d_target_;
// Dynamic reconfigure
Vec3<double> ee_pos_world_target_;
Quat<double> ee_quat_world_target_;
// bool b_is_marker_mode_ = false;
// ros::Subscriber sub_interactive_marker_;
// std::mutex target_ee_pose_mutex_;
//* ----- Control Parameters ---------------------------------------------------------------------
/* Dynamic Reconfigure Server */
std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>
dynamic_server_compliance_param_;
ros::NodeHandle dynamic_reconfigure_compliance_param_node_;
void complianceParamCallback(franka_example_controllers::compliance_paramConfig & config,
uint32_t level);
// Equilibrium pose subscriber
ros::Subscriber sub_equilibrium_pose_;
void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr & msg);
// double nullspace_stiffness_{ 20.0 };
// double nullspace_stiffness_target_{ 20.0 };
/* Target EE Pose command */
// bool b_is_marker_mode_ = false;
// ros::Subscriber sub_interactive_marker_;
// std::mutex target_ee_pose_mutex_;
// Vec3<double> ee_pos_world_des_target_;
// Quat<double> ee_quat_world_des_target_;
// Eigen::Matrix<double, 6, 6> cartesian_stiffness_;
// Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_;
// Eigen::Matrix<double, 6, 6> cartesian_damping_;
// Eigen::Matrix<double, 6, 6> cartesian_damping_target_;
/* spacenav target twist subscriber */
ros::Subscriber sub_target_twist_;
void subscribe_target_twist(const geometry_msgs::TwistConstPtr & msg);
// Vec6<double> ee_vel_world_target_;
// TODO: make it as rosparam or dynamic_reconfigure
Vec6<double> ee_vel_scale_{ 0.1, 0.1, 0.1, 0.1, 0.1, 0.1 };
Vec6<double> ee_vel_world_des_, ee_vel_world_des_prev_;
Vec6<double> ee_acc_world_des_, ee_acc_world_des_prev_;
Mat6<double> K_task_, K_task_target_;
Mat6<double> D_task_, D_task_target_;
Mat7<double> K_null_, K_null_target_;
Mat7<double> D_null_, D_null_target_;
Eigen::Matrix<double, 7, 1> q_d_nullspace_;
//* ----- Control States -------------------------------------------------------------------------
/* Robot Model */
// NOTE: you need to specify absolute path (not relative) for URDF file.
std::string urdf_filename_ =
std::string("/home/ipk410/nix-project/impedance_controller/assets/model/fr3_franka_hand.urdf");
pinocchio::Model model_;
pinocchio::Data data_;
int ee_id_;
// Eigen::Vector3d position_d_;
// Eigen::Quaterniond orientation_d_;
/* Joint states */
Vec7<double> q_mes_;
Vec7<double> dq_mes_, dq_mes_prev_;
Vec7<double> ddq_eul_, ddq_eul_prev_; // joint accel obtained by backward Euler method
Vec7<double> h_mes_, h_mes_prev_; // General Momentum
/* 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_;
Vec3<double> ee_rpy_world_mes_, ee_rpy_world_mes_prev_;
Vec6<double> ee_vel_world_des_, ee_vel_world_des_prev_;
Vec6<double> ee_acc_world_des_, ee_acc_world_des_prev_;
// TODO: make it as rosparam
Vec6<double> fr3_vel_limit_{ 3.0, 3.0, 3.0, 2.5, 2.5, 2.5 }; // [m(rad)/s]
Vec6<double> fr3_acc_limit_{ 9.0, 9.0, 9.0, 17.0, 17.0, 17.0 }; // [m(rad)/s^2]
Vec6<double> fr3_jrk_limit_{ 4500.0, 4500.0, 4500.0, 8500.0, 8500.0, 8500.0 }; // [m(rad)/s^3]
/* 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_cal_, ee_val_world_cal_prev_; // v = J * dq
Vec6<double> ee_acc_world_eul_; // time-derivative using backward Euler method
/* Controllers */
std::unique_ptr<ImpedanceController<double>> imp_;
// Mat6<double> K_task_target_, K_task_;
// Mat6<double> B_task_target_, B_task_;
// Mat7<double> K_null_target_, K_null_;
// Mat7<double> B_null_target_, B_null_;
// Vec7<double> q_null_des_;
std::unique_ptr<DisturbanceObserver<double>> dob_;
// Vec6<double> fq_;
// Vec6<double> Fd_hat_dob_;
std::unique_ptr<SlidingModeObserver<double>> sosml_;
// Vec7<double> s1_, s2_, t1_, t2_;
std::unique_ptr<MomentumObserver<double>> mob_;
// Vec7<double> tau_d_hat_mob_;
// Vec7<double> ddq_hat_mob_, ddq_hat_mob_prev_;
// Vec7<double> r_th_, km_;
// Vec7<double> ddq_hat_mob_filtered_, ddq_hat_mob_filtered_prev_;
/* Joint friction compensation */
// const std::vector<double> fric_phi1{
// 0.54615, 0.87224, 0.64068, 1.2794, 0.83904, 0.30301, 0.56489
// };
// const std::vector<double> fric_phi2{ 5.1181, 9.0657, 10.136, 5.590, 8.3469, 17.133, 10.336 };
// const std::vector<double> fric_phi3{ 0.039533, 0.025882, -0.04607, 0.036194,
// 0.026226, -0.021047, 0.0035526 };
// const std::vector<double> fric_phi3{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
ros::Subscriber sub_joy_;
public:
//* ----- SUBSCRIBE CALLBACK FUNC ----------------------------------------------------------------
void subscribe_target_twist(const geometry_msgs::TwistConstPtr & msg);
void subscribe_joy_command(const sensor_msgs::JoyConstPtr & msg);
std::array<int, 2> btn_cmd_;
/* Kinematics & Dynamics */
// NOTE: you need to specify absolute path (not relative) for URDF file.
std::string urdf_filename_ = std::string("/home/ipk410/nix-project/impedance_controller/assets/model/fr3_franka_hand.urdf");
pinocchio::Model model_;
pinocchio::Data data_;
int ee_id_;
void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr & msg);
//* ----- PARAMETER SERVER CALLBACK FUNC ---------------------------------------------------------
void complianceParamCallback(franka_example_controllers::compliance_paramConfig & config,
uint32_t level);
//* ----- METHODS --------------------------------------------------------------------------------
// Saturation
/* 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)
void init_states();
void update_previous_states();
//* ----- PRINTER --------------------------------------------------------------------------------
void print_end_effector_states();
};
} // namespace franka_example_controllers

Datenschutz und Impressum