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
1 unresolved thread
1 file
+ 4
5
Compare changes
  • Side-by-side
  • Inline
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <franka_example_controllers/cartesian_impedance_example_controller.h>
#include <cmath>
#include <iomanip>
#include <memory>
#include <controller_interface/controller_base.h>
#include <franka/robot_state.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <franka_example_controllers/pseudo_inversion.h>
/* Custom libraries */
#include "orientation_utilities.hpp"
#include "robotics_utilities.hpp"
namespace pin = pinocchio;
namespace ori = orient_utils;
namespace robo = robot_utils;
using namespace std;
// using pin::ReferenceFrame;
namespace franka_example_controllers
{
bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW * robot_hw,
ros::NodeHandle & node_handle)
{
//* Load URDF model and data *//
if (urdf_filename_.empty())
{
throw std::runtime_error("URDF filename is invalid");
}
pin::urdf::buildModel(urdf_filename_, model_);
data_ = pin::Data(model_);
ee_id_ = model_.getFrameId("fr3_hand_tcp", (pin::FrameType::BODY));
//* Set Publisher *//
pub_ctrl_state_ = node_handle.advertise<fr3_custom_msgs::ControllerState>("controller_state", 20);
//* Set Subscriber *//
// NOTE: you need to change topic name as `/spacenav/joy` if you want to command gripper
sub_joy_ = node_handle.subscribe<sensor_msgs::Joy>(
"/joy", 1, &CartesianImpedanceExampleController::subscribe_joy_command, this);
// FIXME: spacenav is not operating !!
// sub_target_twist_ = node_handle.subscribe<geometry_msgs::Twist>(
// "/spacenav/twist", 1, &CartesianImpedanceExampleController::subscribe_target_twist, this);
//* Set HW interface *//
std::string arm_id;
if (!node_handle.getParam("arm_id", arm_id))
{
ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id");
return false;
}
std::vector<std::string> joint_names;
if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7)
{
ROS_ERROR(
"CartesianImpedanceExampleController: Invalid or no joint_names parameters provided, "
"aborting controller init!");
return false;
}
auto * model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
if (model_interface == nullptr)
{
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Error getting model interface from hardware");
return false;
}
try
{
model_handle_ =
std::make_unique<franka_hw::FrankaModelHandle>(model_interface->getHandle(arm_id + "_model"));
}
catch (hardware_interface::HardwareInterfaceException & ex)
{
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Exception getting model handle from interface: "
<< ex.what());
return false;
}
auto * state_interface = robot_hw->get<franka_hw::FrankaStateInterface>();
if (state_interface == nullptr)
{
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Error getting state interface from hardware");
return false;
}
try
{
state_handle_ =
std::make_unique<franka_hw::FrankaStateHandle>(state_interface->getHandle(arm_id + "_robot"));
}
catch (hardware_interface::HardwareInterfaceException & ex)
{
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Exception getting state handle from interface: "
<< ex.what());
return false;
}
auto * effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>();
if (effort_joint_interface == nullptr)
{
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Error getting effort joint interface from hardware");
return false;
}
for (size_t i = 0; i < 7; ++i)
{
try
{
joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
}
catch (const hardware_interface::HardwareInterfaceException & ex)
{
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Exception getting joint handles: " << ex.what());
return false;
}
}
//* Initialize states *//
this->init_states(); // NOTE: this should be executed aforehand of parameter callback
//* Parameter server declaration *//
ctrl_param_node_ = ros::NodeHandle(node_handle.getNamespace() + "/controller_param_node");
ctrl_param_server_ = std::make_unique<
dynamic_reconfigure::Server<franka_example_controllers::controller_paramConfig>>(
ctrl_param_node_);
ctrl_param_server_->setCallback(
boost::bind(&CartesianImpedanceExampleController::request_controller_parameters, this, _1, _2));
//* Create controllers *//
imp_ = std::make_unique<ImpedanceController<double>>();
dob_ = std::make_unique<DisturbanceObserver<double>>(fq_);
mob_ = std::make_unique<MomentumObserver<double>>(r_th_, km_);
sosml_ = std::make_unique<SlidingModeObserver<double>>(s1_, s2_, t1_, t2_);
return true;
}
void CartesianImpedanceExampleController::starting(const ros::Time & /*time*/)
{
// compute initial velocity with jacobian and set x_attractor
franka::RobotState initial_state = state_handle_->getRobotState();
Eigen::Map<Vec7<double>> q_init(initial_state.q.data());
Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
/* set equilibrium pose to current state */
ee_pos_world_des_ = initial_transform.translation();
ee_quat_world_des_ = Eigen::Quaterniond(initial_transform.rotation());
/* set nullspace equilibrium configuration to initial q */
q_null_des_ = q_init;
}
void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
const ros::Duration & period)
{
if (b_count_loop_time_ == true)
{
loop_timer_.start();
}
// cout << std::fixed;
// cout << std::setprecision(5);
// TODO: add safety logic for time derivative when dt is very small
// double dt = period.toSec(); // timestep
double dt = 0.001;
//* ----- set Desired EE Pose & Twist ------------------------------------------------------------
// TODO: Add safety for FR3 end-effector's vel, acc, jerk limits
Vec3<double> v_des = ee_vel_world_des_.head(3);
Vec3<double> w_des = ee_vel_world_des_.tail(3);
ee_pos_world_des_ += v_des * dt;
ee_quat_world_des_ = ori::getNewQuat(ee_quat_world_des_, w_des, dt);
// ee_rpy_world_des_ = ori::quaternionToRPY(ee_quat_world_des_, ee_rpy_world_des_prev_);
//* ----- get Current States --------------------------------------------------------------------
franka::RobotState robot_state = state_handle_->getRobotState();
Eigen::Map<Vec7<double>> tau_J_d(robot_state.tau_J_d.data()); // NOLINT
Eigen::Map<const Vec6<double>> tau_ext_hat_filtered(robot_state.tau_ext_hat_filtered.data());
q_mes_ = Eigen::Map<const Vec7<double>>(robot_state.q.data());
dq_mes_ = Eigen::Map<const Vec7<double>>(robot_state.dq.data());
// TODO: check noise level
// ddq_eul_ = robo::getEulerDerivative(dq_mes_, dq_mes_prev_, dt);
ddq_eul_ = robo::getFilteredEulerDerivative(dq_mes_, dq_mes_prev_, ddq_eul_prev_, dt, 10.0);
pin::forwardKinematics(model_, data_, q_mes_, dq_mes_);
pin::updateFramePlacements(model_, data_);
ee_pos_world_mes_ = data_.oMf[ee_id_].translation();
RotMat<double> R_OE = data_.oMf[ee_id_].rotation();
ee_quat_world_mes_ = Quat<double>(R_OE);
// ee_rpy_world_mes_ = ori::quaternionToRPY(ee_quat_world_mes_, ee_rpy_world_mes_prev_);
//* ----- compute Kinematics & Dynamics ----------------------------------------------------------
/* Jacobian */
Mat67<double> Je, dJe;
pin::getFrameJacobian(model_, data_, ee_id_, pin::ReferenceFrame::LOCAL_WORLD_ALIGNED, Je);
pin::getFrameJacobianTimeVariation(model_, data_, ee_id_,
pin::ReferenceFrame::LOCAL_WORLD_ALIGNED, dJe);
Mat76<double> Je_t = Je.transpose();
MatX<double> Je_pinv, Je_t_pinv;
pseudoInverse(Je, Je_pinv);
pseudoInverse(Je_t, Je_t_pinv);
// NOTE: using `pseudoInverse()` might be more safe against singularities
// MatX<double> Je_pinv = Je.completeOrthogonalDecomposition().pseudoInverse();
// MatX<double> Je_t_pinv = Je.transpose().completeOrthogonalDecomposition().pseudoInverse();
/* compute current EE Twist & Acceleration using backward Euler method */
ee_vel_world_mes_ = Je * dq_mes_;
// ee_acc_world_eul_ = robo::getEulerDerivative(ee_vel_world_mes_, ee_vel_world_mes_prev_, dt);
ee_acc_world_eul_ = robo::getFilteredEulerDerivative(ee_vel_world_mes_, ee_vel_world_mes_prev_, ee_acc_world_eul_prev_, dt, 10.0);
/* Inertia matrix */
pin::crba(model_, data_, q_mes_);
// Mat7<double> M = data_.M; // upper triangular matrix in joint-space
// Vec7<double> k_mass;
// k_mass << 1.0, 1.0, 1.0, 1.0, 10.0, 10.0, 50.0;
// M.diagonal().array() *= k_mass.array();
Mat7<double> M = robo::computeFullInertiaMatrix(data_.M);
Mat6<double> Me = robo::computeTaskInertiaMatrix(M, Je_pinv, Je_t_pinv);
/* Coriolis, Centrifugal & Gravity torque */
pin::computeCoriolisMatrix(model_, data_, q_mes_, dq_mes_);
Mat7<double> C = data_.C;
Vec7<double> tau_c = C * dq_mes_;
// pin::computeGeneralizedGravity(model_, data_, q_mes_); // * gravity is compensated internally
// Vec7<double> tau_g = data_.g;
Vec7<double> tau_g;
tau_g.setZero();
//* ----- CONTROL LAW ----------------------------------------------------------------------------
/* Cartesian impedance control */
imp_->set_parameters(K_task_, D_task_, K_null_, D_null_);
Vec6<double> ee_pose_err = imp_->get_pose_error(ee_pos_world_des_, ee_pos_world_mes_,
ee_quat_world_des_, ee_quat_world_mes_);
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);
/* 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 */
imp_->compute_null_error(q_null_des_, q_mes_);
Vec7<double> tau_null = imp_->get_null_impedance_torque(dq_mes_, Je_t, Je_t_pinv);
/* compute desired joint torque command */
Vec7<double> tau_des = tau_task + tau_null + tau_c;
tau_des << saturateTorqueRate(tau_des, tau_J_d); // Saturate torque rate to avoid discontinuities
//* ----- Observers ------------------------------------------------------------------------------
Fd_hat_fr_ = Je_t_pinv * tau_ext_hat_filtered;
/* Momentum Observer (MOB) */
h_mes_ = M * dq_mes_;
Vec7<double> dh_mes = robo::getEulerDerivative(h_mes_, h_mes_prev_, dt);
mob_->set_parameters(dt, r_th_, km_); // TODO: set parameter similar level to DOB for comparison
mob_->set_dynamics(tau_des, M, C, tau_g);
mob_->compute_residual(dh_mes, dq_mes_);
ddq_hat_mob_ = mob_->get_joint_accel_estimation(dq_mes_);
ee_acc_world_hat_mob_ = robo::getTaskAcceleration(Je, dJe, dq_mes_, ddq_hat_mob_);
tau_d_hat_mob_ = mob_->get_external_torque_estimation();
Fd_hat_mob_ = Je_t_pinv * tau_d_hat_mob_;
/* 2nd-order Sliding mode Observer (SOSML) */
sosml_->set_parameters(dt, s1_, s2_, t1_, t2_);
sosml_->set_dynamics(tau_des, M, C, tau_g);
sosml_->compute_momentum_estimation(dq_mes_);
ddq_hat_sosml_ = sosml_->get_joint_accel_estimation();
ee_acc_world_hat_sosml_ = robo::getTaskAcceleration(Je, dJe, dq_mes_, ddq_hat_sosml_);
tau_d_hat_sosml_ = sosml_->get_external_torque_estimation();
Fd_hat_sosml_ = Je_t_pinv * tau_d_hat_sosml_;
/* Task-space Force Disturbance Observer (TFDOB) */
dob_->set_parameters(dt, fq_);
Vec6<double> ddx = (b_sosml_applied_ ? ee_acc_world_hat_sosml_ : ee_acc_world_eul_);
Fd_hat_dob_ = dob_->get_force_disturbance(F_task, Me, ddx);
tau_d_hat_dob_ = Je_t * Fd_hat_dob_;
//* send joint torque command *//
for (size_t i = 0; i < 7; ++i)
{
joint_handles_[i].setCommand(tau_des(i));
}
this->publish_controller_state(); // publish states for rosbag
//* update previous states *//
this->update_previous_states();
imp_->update_previous_states();
dob_->update_previous_states();
//* update parameters online either via dynamic reconfigure or target by filtering *//
/* update desired twist via joystick */
ee_vel_world_des_ = alpha_ * ee_vel_world_target_ + (1.0 - alpha_) * ee_vel_world_des_;
/* update controller parameters */
K_task_ = alpha_ * K_task_target_ + (1.0 - alpha_) * K_task_;
D_task_ = alpha_ * D_task_target_ + (1.0 - alpha_) * D_task_;
K_null_ = alpha_ * K_null_target_ + (1.0 - alpha_) * K_null_;
D_null_ = alpha_ * D_null_target_ + (1.0 - alpha_) * D_null_;
fq_ = alpha_ * fq_target_ + (1.0 - alpha_) * fq_;
s1_ = alpha_ * s1_target_ + (1.0 - alpha_) * s1_;
s2_ = alpha_ * s2_target_ + (1.0 - alpha_) * s2_;
t1_ = s1_.array().sqrt();
t2_ = s2_.array().sqrt();
r_th_ = alpha_ * r_th_target_ + (1.0 - alpha_) * r_th_;
km_ = alpha_ * km_target_ + (1.0 - alpha_) * km_;
if(b_count_loop_time_ == true)
{
if(max_exec_time_ < loop_timer_.getMillisec())
{
max_exec_time_ = loop_timer_.getMillisec();
}
cout << "[Execution time] " << loop_timer_.getMillisec() << "(msec) [Max] " << max_exec_time_ << endl;
}
}
//* ----- SUBSCRIBE CALLBACK -----------------------------------------------------------------------
void CartesianImpedanceExampleController::subscribe_target_twist(
const geometry_msgs::TwistConstPtr & msg)
{
// cout << msg->linear.x << ", " << msg->linear.y << ", " << msg->linear.z << endl;
ee_vel_world_des_[0] = twist_scale_[0] * msg->linear.x;
ee_vel_world_des_[1] = twist_scale_[1] * msg->linear.y;
ee_vel_world_des_[2] = twist_scale_[2] * msg->linear.z;
ee_vel_world_des_[3] = twist_scale_[3] * msg->angular.x;
ee_vel_world_des_[4] = twist_scale_[4] * msg->angular.y;
ee_vel_world_des_[5] = twist_scale_[5] * msg->angular.z;
}
void CartesianImpedanceExampleController::subscribe_joy_command(
const sensor_msgs::JoyConstPtr & msg)
{
// btn_cmd_[0] = msg->buttons[0];
// btn_cmd_[1] = msg->buttons[1];
ee_vel_world_target_[0] = static_cast<double>(twist_scale_[0] * msg->axes[0]); // x
ee_vel_world_target_[1] = static_cast<double>(twist_scale_[1] * msg->axes[1]); // y
ee_vel_world_target_[2] = static_cast<double>(twist_scale_[2] * msg->axes[5]); // z
ee_vel_world_target_[3] = static_cast<double>(twist_scale_[3] * msg->axes[2]); // roll
ee_vel_world_target_[4] = static_cast<double>(twist_scale_[4] * msg->axes[3]); // pitch
ee_vel_world_target_[5] = static_cast<double>(twist_scale_[5] * msg->axes[4]); // yaw
}
//* ----- PARAMETER SERVER CALLBACK ----------------------------------------------------------------
void CartesianImpedanceExampleController::request_controller_parameters(
franka_example_controllers::controller_paramConfig & config, uint32_t /*level*/)
{
//* Impedance Controller (NOTE: Damping ratio = 1) *//
/* task-space PD gains */
K_task_target_.setIdentity().diagonal() << config.K_task_x, config.K_task_y, config.K_task_z, //
config.K_task_rot, config.K_task_rot, config.K_task_rot;
D_task_target_.setIdentity().diagonal() << 2.0 * K_task_target_.diagonal().array().sqrt();
/* null-space PD gains */
K_null_target_.setIdentity().diagonal() << config.K_null, config.K_null, config.K_null, //
config.K_null, config.K_null, config.K_null, config.K_null;
D_null_target_.setIdentity().diagonal() << 2.0 * K_null_target_.diagonal().array().sqrt();
//* Disturbance Observer *//
fq_target_ << config.fq, config.fq, config.fq, config.fq, config.fq, config.fq;
//* Sliding Mode Observer *//
s1_target_ << config.s1, config.s1, config.s1, config.s1, config.s1, config.s1, config.s1;
s2_target_ << config.s2, config.s2, config.s2, config.s2, config.s2, config.s2, config.s2;
//* Momentum Observer *//
r_th_target_ << config.r_th, config.r_th, config.r_th, config.r_th, config.r_th, config.r_th,
config.r_th;
km_target_ << config.Km, config.Km, config.Km, config.Km, config.Km, config.Km, config.Km;
}
//* ----- METHODS ----------------------------------------------------------------------------------
void CartesianImpedanceExampleController::init_states()
{
//* subscriber *//
ee_vel_world_target_.setZero();
//* control parameters *//
/* impedance */
K_task_.setIdentity().diagonal() << 200.0, 200.0, 200.0, 10.0, 10.0, 10.0; // TODO: set default
K_task_target_.setIdentity();
D_task_.setIdentity().diagonal() << 2.0 * K_task_.diagonal().array().sqrt();
D_task_target_.setIdentity();
K_null_.setIdentity().diagonal() << 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5;
K_null_target_.setIdentity();
D_null_.setIdentity().diagonal() << 2.0 * K_null_.diagonal().array().sqrt();
D_null_target_.setIdentity();
/* DOB */
fq_ << 15.0, 15.0, 15.0, 15.0, 15.0, 15.0;
fq_target_.setZero();
/* SOSML */
s1_ << 10, 10, 10, 10, 10, 10, 10;
s1_target_.setZero();
s2_ << 200, 200, 200, 200, 200, 200, 200;
s2_target_.setZero();
t1_ = s1_.array().sqrt();
t1_target_.setZero();
t2_ = s2_.array().sqrt();
t2_target_.setZero();
/* MOB */
r_th_ << 0, 0, 0, 0, 0, 0, 0;
r_th_target_.setZero();
km_ << 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5;
km_target_.setZero();
//* Control states *//
q_mes_.setZero();
dq_mes_.setZero();
dq_mes_prev_.setZero();
ddq_eul_.setZero();
ddq_eul_prev_.setZero();
q_null_des_.setZero();
ee_pos_world_des_.setZero();
ee_quat_world_des_.coeffs() << 0.0, 0.0, 0.0, 1.0;
ee_rpy_world_des_.setZero();
ee_rpy_world_des_prev_.setZero();
ee_vel_world_des_.setZero();
ee_vel_world_des_prev_.setZero();
ee_acc_world_des_.setZero();
ee_acc_world_des_prev_.setZero();
ee_pos_world_mes_.setZero();
ee_quat_world_mes_.coeffs() << 0.0, 0.0, 0.0, 1.0;
ee_rpy_world_mes_.setZero();
ee_rpy_world_mes_prev_.setZero();
ee_vel_world_mes_.setZero();
ee_vel_world_mes_prev_.setZero();
ee_acc_world_eul_.setZero();
ee_acc_world_eul_prev_.setZero();
tau_d_hat_dob_.setZero();
Fd_hat_dob_.setZero();
ddq_hat_sosml_.setZero();
tau_d_hat_sosml_.setZero();
ee_acc_world_hat_sosml_.setZero();
Fd_hat_sosml_.setZero();
h_mes_.setZero();
h_mes_prev_.setZero();
ddq_hat_mob_.setZero();
ee_acc_world_hat_mob_.setZero();
tau_d_hat_mob_.setZero();
Fd_hat_mob_.setZero();
Fd_hat_fr_.setZero();
}
void CartesianImpedanceExampleController::publish_controller_state()
{
for (int i = 0; i < 7; ++i)
{
msg_ctrl_state_.ddq_eul[i] = ddq_eul_[i];
msg_ctrl_state_.ddq_mob[i] = ddq_hat_mob_[i];
msg_ctrl_state_.ddq_sosml[i] = ddq_hat_sosml_[i];
msg_ctrl_state_.tau_dist_dob[i] = tau_d_hat_dob_[i];
msg_ctrl_state_.tau_dist_mob[i] = tau_d_hat_mob_[i];
msg_ctrl_state_.tau_dist_sosml[i] = tau_d_hat_sosml_[i];
}
msg_ctrl_state_.F_dist_franka.force.x = Fd_hat_fr_[0];
msg_ctrl_state_.F_dist_franka.force.y = Fd_hat_fr_[1];
msg_ctrl_state_.F_dist_franka.force.z = Fd_hat_fr_[2];
msg_ctrl_state_.F_dist_franka.torque.x = Fd_hat_fr_[3];
msg_ctrl_state_.F_dist_franka.torque.y = Fd_hat_fr_[4];
msg_ctrl_state_.F_dist_franka.torque.z = Fd_hat_fr_[5];
msg_ctrl_state_.F_dist_dob.force.x = Fd_hat_dob_[0];
msg_ctrl_state_.F_dist_dob.force.y = Fd_hat_dob_[1];
msg_ctrl_state_.F_dist_dob.force.z = Fd_hat_dob_[2];
msg_ctrl_state_.F_dist_dob.torque.x = Fd_hat_dob_[3];
msg_ctrl_state_.F_dist_dob.torque.y = Fd_hat_dob_[4];
msg_ctrl_state_.F_dist_dob.torque.z = Fd_hat_dob_[5];
msg_ctrl_state_.F_dist_mob.force.x = Fd_hat_mob_[0];
msg_ctrl_state_.F_dist_mob.force.y = Fd_hat_mob_[1];
msg_ctrl_state_.F_dist_mob.force.z = Fd_hat_mob_[2];
msg_ctrl_state_.F_dist_mob.torque.x = Fd_hat_mob_[3];
msg_ctrl_state_.F_dist_mob.torque.y = Fd_hat_mob_[4];
msg_ctrl_state_.F_dist_mob.torque.z = Fd_hat_mob_[5];
/* task-space pose */
msg_ctrl_state_.ee_pose_world_des.position.x = ee_pos_world_des_[0];
msg_ctrl_state_.ee_pose_world_des.position.y = ee_pos_world_des_[1];
msg_ctrl_state_.ee_pose_world_des.position.z = ee_pos_world_des_[2];
msg_ctrl_state_.ee_pose_world_des.orientation.x = ee_quat_world_des_.x();
msg_ctrl_state_.ee_pose_world_des.orientation.y = ee_quat_world_des_.y();
msg_ctrl_state_.ee_pose_world_des.orientation.z = ee_quat_world_des_.z();
msg_ctrl_state_.ee_pose_world_des.orientation.w = ee_quat_world_des_.w();
msg_ctrl_state_.ee_pose_world_mes.position.x = ee_pos_world_mes_[0];
msg_ctrl_state_.ee_pose_world_mes.position.y = ee_pos_world_mes_[1];
msg_ctrl_state_.ee_pose_world_mes.position.z = ee_pos_world_mes_[2];
msg_ctrl_state_.ee_pose_world_mes.orientation.x = ee_quat_world_mes_.x();
msg_ctrl_state_.ee_pose_world_mes.orientation.y = ee_quat_world_mes_.y();
msg_ctrl_state_.ee_pose_world_mes.orientation.z = ee_quat_world_mes_.z();
msg_ctrl_state_.ee_pose_world_mes.orientation.w = ee_quat_world_mes_.w();
/* task-space twist */
msg_ctrl_state_.ee_vel_world_des.linear.x = ee_vel_world_des_[0];
msg_ctrl_state_.ee_vel_world_des.linear.y = ee_vel_world_des_[1];
msg_ctrl_state_.ee_vel_world_des.linear.z = ee_vel_world_des_[2];
msg_ctrl_state_.ee_vel_world_des.angular.x = ee_vel_world_des_[3];
msg_ctrl_state_.ee_vel_world_des.angular.y = ee_vel_world_des_[4];
msg_ctrl_state_.ee_vel_world_des.angular.z = ee_vel_world_des_[5];
msg_ctrl_state_.ee_vel_world_mes.linear.x = ee_vel_world_mes_[0];
msg_ctrl_state_.ee_vel_world_mes.linear.y = ee_vel_world_mes_[1];
msg_ctrl_state_.ee_vel_world_mes.linear.z = ee_vel_world_mes_[2];
msg_ctrl_state_.ee_vel_world_mes.angular.x = ee_vel_world_mes_[3];
msg_ctrl_state_.ee_vel_world_mes.angular.y = ee_vel_world_mes_[4];
msg_ctrl_state_.ee_vel_world_mes.angular.z = ee_vel_world_mes_[5];
/* task-space acceleration */
msg_ctrl_state_.ee_acc_world_eul.linear.x = ee_acc_world_eul_[0];
msg_ctrl_state_.ee_acc_world_eul.linear.y = ee_acc_world_eul_[1];
msg_ctrl_state_.ee_acc_world_eul.linear.z = ee_acc_world_eul_[2];
msg_ctrl_state_.ee_acc_world_eul.angular.x = ee_acc_world_eul_[3];
msg_ctrl_state_.ee_acc_world_eul.angular.y = ee_acc_world_eul_[4];
msg_ctrl_state_.ee_acc_world_eul.angular.z = ee_acc_world_eul_[5];
msg_ctrl_state_.ee_acc_world_mob.linear.x = ee_acc_world_hat_mob_[0];
msg_ctrl_state_.ee_acc_world_mob.linear.y = ee_acc_world_hat_mob_[1];
msg_ctrl_state_.ee_acc_world_mob.linear.z = ee_acc_world_hat_mob_[2];
msg_ctrl_state_.ee_acc_world_mob.angular.x = ee_acc_world_hat_mob_[3];
msg_ctrl_state_.ee_acc_world_mob.angular.y = ee_acc_world_hat_mob_[4];
msg_ctrl_state_.ee_acc_world_mob.angular.z = ee_acc_world_hat_mob_[5];
msg_ctrl_state_.ee_acc_world_sosml.linear.x = ee_acc_world_hat_sosml_[0];
msg_ctrl_state_.ee_acc_world_sosml.linear.y = ee_acc_world_hat_sosml_[1];
msg_ctrl_state_.ee_acc_world_sosml.linear.z = ee_acc_world_hat_sosml_[2];
msg_ctrl_state_.ee_acc_world_sosml.angular.x = ee_acc_world_hat_sosml_[3];
msg_ctrl_state_.ee_acc_world_sosml.angular.y = ee_acc_world_hat_sosml_[4];
msg_ctrl_state_.ee_acc_world_sosml.angular.z = ee_acc_world_hat_sosml_[5];
pub_ctrl_state_.publish(msg_ctrl_state_);
}
void CartesianImpedanceExampleController::update_previous_states()
{
dq_mes_prev_ = dq_mes_;
ddq_eul_prev_ = ddq_eul_;
ee_rpy_world_des_prev_ = ee_rpy_world_des_;
ee_vel_world_des_prev_ = ee_vel_world_des_;
ee_acc_world_des_prev_ = ee_acc_world_des_;
ee_rpy_world_mes_prev_ = ee_rpy_world_mes_;
ee_vel_world_mes_prev_ = ee_vel_world_mes_;
ee_acc_world_eul_prev_ = ee_acc_world_eul_;
h_mes_prev_ = h_mes_;
}
Eigen::Matrix<double, 7, 1> CartesianImpedanceExampleController::saturateTorqueRate(
const Eigen::Matrix<double, 7, 1> & tau_d_calculated, const Eigen::Matrix<double, 7, 1> & tau_J_d)
{ // NOLINT (readability-identifier-naming)
Eigen::Matrix<double, 7, 1> tau_d_saturated{};
for (size_t i = 0; i < 7; i++)
{
double difference = tau_d_calculated[i] - tau_J_d[i];
tau_d_saturated[i] =
tau_J_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_);
}
return tau_d_saturated;
}
//* ----- PRINTER ----------------------------------------------------------------------------------
void CartesianImpedanceExampleController::print_end_effector_states()
{
// cout << "Target EE twist:\t" << ee_vel_world_target_.transpose() << endl;
// cout << "Desired EE twist:\t" << ee_vel_world_des_.transpose() << endl;
// 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;
cout << "Desired EE pos [m]:\t" << ee_pos_world_des_.transpose() << endl;
cout << "Desired EE RPY [rad]:\t" << ee_rpy_world_des_.transpose() << endl;
cout << "Measured EE RPY [rad]:\t" << ee_rpy_world_mes_.transpose() << endl << 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;
}
void CartesianImpedanceExampleController::print_dynamics()
{
// NOTE: dynamics are a little bit different
// cout << "FR3:\n" << M_fr << endl;
// cout << "Pin:\n" << M << endl;
// cout << "Coriolis FR3:\t" << coriolis.transpose() << endl;
// cout << "Coriolis Pin:\t" << tau_c.transpose() << endl;
}
} // namespace franka_example_controllers
PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianImpedanceExampleController,
controller_interface::ControllerBase)
Loading