#include <CartesianImpedance.hh>
Public Member Functions | |
CartesianImpedanceControl () | |
tFriCmdData | computeRobotCommand (const tFriMsrData &state, const tFriCmdData &cmd, const CartesianImpedance &imp, const float *cmdpos) |
~CartesianImpedanceControl () | |
Private Member Functions | |
template<typename T1 , typename T2 > | |
void | pinv (Eigen::DenseBase< T1 > &J, Eigen::DenseBase< T2 > &Jinv, double eps=1e-13) |
Private Attributes | |
KDL::Chain | chain_ |
KDL::Jacobian | J_kdl_ |
Eigen::Matrix< float, 7, 6 > | Jinv |
KDL::ChainJntToJacSolver * | jnt2jac_ |
KDL::JntArray | q_kdl_ |
float | q_last_ [7] |
Eigen::VectorXd | S |
Eigen::VectorXd | Sp |
Eigen::VectorXd | tmp |
Eigen::MatrixXd | U |
Eigen::MatrixXd | V |
Definition at line 23 of file CartesianImpedance.hh.
Definition at line 32 of file CartesianImpedance.cc.
Definition at line 57 of file CartesianImpedance.cc.
tFriCmdData CartesianImpedanceControl::computeRobotCommand | ( | const tFriMsrData & | fri_msr, |
const tFriCmdData & | fri_cmd, | ||
const CartesianImpedance & | imp, | ||
const float * | cmdpos | ||
) |
adds torques to cmd.addTorque and replaces cmd.stiffness, cmd.damping and cmd.command
Definition at line 65 of file CartesianImpedance.cc.
void CartesianImpedanceControl::pinv | ( | Eigen::DenseBase< T1 > & | J, |
Eigen::DenseBase< T2 > & | Jinv, | ||
double | eps = 1e-13 |
||
) | [private] |
Definition at line 131 of file CartesianImpedance.cc.
KDL::Chain CartesianImpedanceControl::chain_ [private] |
Definition at line 39 of file CartesianImpedance.hh.
Definition at line 41 of file CartesianImpedance.hh.
Eigen::Matrix<float, 7, 6> CartesianImpedanceControl::Jinv [private] |
Definition at line 46 of file CartesianImpedance.hh.
Definition at line 40 of file CartesianImpedance.hh.
KDL::JntArray CartesianImpedanceControl::q_kdl_ [private] |
Definition at line 38 of file CartesianImpedance.hh.
float CartesianImpedanceControl::q_last_[7] [private] |
Definition at line 36 of file CartesianImpedance.hh.
Eigen::VectorXd CartesianImpedanceControl::S [private] |
Definition at line 47 of file CartesianImpedance.hh.
Eigen::VectorXd CartesianImpedanceControl::Sp [private] |
Definition at line 47 of file CartesianImpedance.hh.
Eigen::VectorXd CartesianImpedanceControl::tmp [private] |
Definition at line 47 of file CartesianImpedance.hh.
Eigen::MatrixXd CartesianImpedanceControl::U [private] |
Definition at line 45 of file CartesianImpedance.hh.
Eigen::MatrixXd CartesianImpedanceControl::V [private] |
Definition at line 45 of file CartesianImpedance.hh.