CartesianImpedance.hh
Go to the documentation of this file.
00001 #ifndef CARTESIAN_IMPEDANCE_HH
00002 #define CARTESIAN_IMPEDANCE_HH
00003 
00004 // This controller implements a cartesian impedance (stiffness and damping)
00005 // on top of joint impedance control.
00006 //
00007 // The allowed inputs are:
00008 //
00009 // q_desired, K_cart, D_cart, ft_cart
00010 //
00011 // The cartesian parameters are expressed in the reference frame of the base
00012 // with the reference point flanche.
00013 
00014 #include "FRIData.hh"
00015 #include <kdl/jacobian.hpp>
00016 #include <kdl/jntarray.hpp>
00017 #include <kdl/chain.hpp>
00018 #include <kdl/chainjnttojacsolver.hpp>
00019 
00020 #include <friComm.h>  // KUKA proprietary header
00021 
00022 
00023 class CartesianImpedanceControl
00024 {
00025 public:
00026 
00027   CartesianImpedanceControl();
00028   ~CartesianImpedanceControl();
00029 
00032   tFriCmdData computeRobotCommand(const tFriMsrData& state, const tFriCmdData& cmd,
00033                                   const CartesianImpedance& imp, const float* cmdpos);
00034 
00035 private:
00036   float q_last_[7];
00037 
00038   KDL::JntArray q_kdl_;
00039   KDL::Chain chain_;
00040   KDL::ChainJntToJacSolver* jnt2jac_;
00041   KDL::Jacobian J_kdl_;
00042 
00043   // helper matrices for Jacobian inversion
00044 
00045   Eigen::MatrixXd U, V;
00046   Eigen::Matrix<float, 7, 6> Jinv;
00047   Eigen::VectorXd S, Sp, tmp;
00048 
00049   template <typename T1, typename T2>
00050   void pinv(Eigen::DenseBase<T1>& J, Eigen::DenseBase<T2>& Jinv, double eps=1e-13);
00051 };
00052 
00053 #endif // CARTESIAN_IMPEDANCE_HH
00054 


kuka_fri
Author(s): Ingo Kresse, Alexis Maldonado
autogenerated on Mon Oct 6 2014 09:27:39