added quaternions

lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp

@@ -10,6 +10,7 @@
 
 #include "controller_interface/controller_interface.hpp"
 #include "eigen3/Eigen/Core"
+#include "eigen3/Eigen/Geometry"
 #include "hardware_interface/loaned_state_interface.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -58,8 +59,9 @@ class AdmittanceController : public controller_interface::ControllerInterface {
   // admittance
   bool initialized_ = false;
   std::unique_ptr<lbr_fri_ros2::AdmittanceImpl> admittance_impl_ptr_;
-  Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> x_init_, x_prev_;
-  Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> f_ext_, x_, dx_, ddx_;
+  Eigen::Matrix<double, 3, 1> t_init_, t_, t_prev_; // translation
+  Eigen::Quaterniond r_init_, r_, r_prev_;          // rotation
+  Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> f_ext_, delta_x_, dx_, ddx_;
 
   // joint veloctiy computation
   std::unique_ptr<lbr_fri_ros2::InvJacCtrlImpl> inv_jac_ctrl_impl_ptr_;

lbr_ros2_control/src/controllers/admittance_controller.cpp

@@ -112,26 +112,38 @@ controller_interface::return_type AdmittanceController::update(const rclcpp::Tim
 
   // compute forward kinematics
   auto chain_tip_frame = inv_jac_ctrl_impl_ptr_->get_kinematics_ptr()->compute_fk(q_);
-  x_.head(3) = Eigen::Map<Eigen::Matrix<double, 3, 1>>(chain_tip_frame.p.data);
-  x_.tail(3) = Eigen::Map<Eigen::Matrix<double, 3, 1>>(chain_tip_frame.M.GetRot().data);
+  t_ = Eigen::Map<Eigen::Matrix<double, 3, 1>>(chain_tip_frame.p.data);
+  r_ = Eigen::Quaterniond(chain_tip_frame.M.data);
 
   // compute steady state position and orientation
   if (!initialized_) {
-    x_init_ = x_;
-    x_prev_ = x_;
+    t_init_ = t_;
+    t_prev_ = t_init_;
+    r_init_ = r_;
+    r_prev_ = r_init_;
     initialized_ = true;
   }
 
-  // compute velocity & update previous position
-  dx_ = (x_ - x_prev_) / period.seconds();
-  x_prev_ = x_;
+  // compute translational delta and velocity
+  delta_x_.head(3) = (t_ - t_init_);
+  dx_.head(3) = (t_ - t_prev_) / period.seconds();
+
+  // compute rotational delta and veloctity
+  Eigen::AngleAxisd deltaa(r_.inverse() * r_init_);
+  delta_x_.tail(4) = deltaa.axis() * deltaa.angle();
+  Eigen::AngleAxisd da(r_.inverse() * r_prev_);
+  dx_.tail(3) = da.axis() * da.angle();
+
+  // update previous values
+  t_prev_ = t_;
+  r_prev_ = r_;
 
   // convert f_ext_ back to root frame
   f_ext_.head(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * f_ext_.head(3);
   f_ext_.tail(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * f_ext_.tail(3);
 
   // compute admittance
-  admittance_impl_ptr_->compute(f_ext_, x_ - x_init_, dx_, ddx_);
+  admittance_impl_ptr_->compute(f_ext_, delta_x_, dx_, ddx_);
 
   // integrate ddx_ to command velocity
   twist_command_ = ddx_ * period.seconds();
@@ -327,7 +339,7 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() {
 
 void AdmittanceController::zero_all_values_() {
   f_ext_.setZero();
-  x_.setZero();
+  delta_x_.setZero();
   dx_.setZero();
   ddx_.setZero();
   std::fill(dq_.begin(), dq_.end(), 0.0);