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
5 files
+ 22
22
Compare changes
  • Side-by-side
  • Inline
Files
5
#include "ImpedanceController.hpp"
#include "orientation_utilities.hpp"
#include <iostream>
namespace ori = orient_utils;
using namespace std;
template <typename T>
ImpedanceController<T>::ImpedanceController(const Mat6<T> & K_task, const Mat6<T> & B_task,
const Mat7<T> & K_null, const Mat7<T> & B_null)
: K_task_(K_task), B_task_(B_task), K_null_(K_null), B_null_(B_null)
ImpedanceController<T>::ImpedanceController()
{
x_0e_err_.setZero();
x_0e_err_prev_.setZero();
@@ -17,9 +19,25 @@ ImpedanceController<T>::ImpedanceController(const Mat6<T> & K_task, const Mat6<T
q_null_err_.setZero();
q_null_err_prev_.setZero();
tau_null_.setZero();
K_task_.setIdentity();
D_task_.setIdentity();
K_null_.setIdentity();
D_null_.setIdentity();
}
//* ----- SETTERS ----------------------------------------------------------------------------------
template <typename T>
void ImpedanceController<T>::set_parameters(const Mat6<T> & K_task, const Mat6<T> & D_task,
const Mat7<T> & K_null, const Mat7<T> & D_null)
{
K_task_ = K_task;
D_task_ = D_task;
K_null_ = K_null;
D_null_ = D_null;
}
//* ----- METHODS ------------------------------------------------------------------------------ *//
//* ----- METHODS ----------------------------------------------------------------------------------
template <typename T>
void ImpedanceController<T>::compute_null_error(const Vec7<T> & q_null_des,
const Vec7<T> & q_null_mes)
@@ -27,7 +45,7 @@ void ImpedanceController<T>::compute_null_error(const Vec7<T> & q_null_des,
q_null_err_ = q_null_des - q_null_mes;
}
//* ----- GETTERS ------------------------------------------------------------------------------ *//
//* ----- GETTERS ----------------------------------------------------------------------------------
template <typename T>
Vec6<T> ImpedanceController<T>::get_pose_error(const Vec3<T> & p_0e_des, const Vec3<T> & p_0e_mes,
const Quat<T> & q_0e_des, const Quat<T> & q_0e_mes)
@@ -44,8 +62,10 @@ Vec6<T> ImpedanceController<T>::get_pose_error(const Vec3<T> & p_0e_des, const V
q_new.coeffs() << q_0e_mes.coeffs();
}
Quat<T> q_0e_err = q_new.inverse() * q_0e_des;
RotMat<T> R = ori::quaternionToRotationMatrix(q_new);
x_0e_err_.tail(3) << q_0e_err.x(), q_0e_err.y(), q_0e_err.z();
x_0e_err_.tail(3) << q_new * x_0e_err_.tail(3);
// x_0e_err_.tail(3) << q_new * x_0e_err_.tail(3);
x_0e_err_.tail(3) << R * x_0e_err_.tail(3);
return x_0e_err_;
}
@@ -62,16 +82,16 @@ Vec6<T> ImpedanceController<T>::get_previous_pose_error()
template <typename T>
Vec6<T> ImpedanceController<T>::get_task_impedance_force(const Vec6<T> & dx_0e_err)
{
return K_task_ * x_0e_err_ + B_task_ * dx_0e_err;
return K_task_ * x_0e_err_ + D_task_ * dx_0e_err;
}
template <typename T>
Vec7<T> ImpedanceController<T>::get_null_impedance_torque(const Vec7<T> & dq_null_err,
const Eigen::MatrixXd & J,
const Eigen::MatrixXd & Jt_pinv)
const MatX<T> & J_t,
const MatX<T> & J_t_pinv)
{
return (Eigen::MatrixXd::Identity(7, 7) - J.transpose() * Jt_pinv) *
(K_null_ * q_null_err_ - B_null_ * dq_null_err);
Mat7<T> I = Mat7<T>::Identity();
return (I - J_t * J_t_pinv) * (K_null_ * q_null_err_ - D_null_ * dq_null_err);
}
//* ----- PRINTER ------------------------------------------------------------------------------ *//
Loading