6 #ifndef _DeviceTorqueCtrl_H_ 7 #define _DeviceTorqueCtrl_H_ 9 #include <pinocchio/fwd.hpp> 13 #include <dynamic-graph/entity.h> 14 #include <dynamic-graph/linear-algebra.h> 15 #include <dynamic-graph/signal-ptr.h> 16 #include <dynamic-graph/signal.h> 18 #include <Eigen/Cholesky> 19 #include <boost/random/mersenne_twister.hpp> 20 #include <boost/random/normal_distribution.hpp> 21 #include <boost/random/variate_generator.hpp> 22 #include <sot/core/abstract-sot-external-interface.hh> 23 #include <sot/core/device.hh> 24 #include <tsid/robots/robot-wrapper.hpp> 25 #include <tsid/tasks/task-se3-equality.hpp> 28 #include <dynamic-graph/signal-helper.h> 30 #include <sot/core/robot-utils.hh> 71 virtual void setState(
const dynamicgraph::Vector& st);
72 virtual void setVelocity(
const dynamicgraph::Vector& vel);
75 virtual void init(
const double& dt,
const std::string& urdfFile);
81 void sendMsg(
const std::string& msg, MsgType t = MSG_TYPE_INFO,
82 const char* =
"",
int = 0) {
83 logger_.stream(t) << (
"[DeviceTorqueCtrl] " + msg) <<
'\n';
91 dynamicgraph::SignalPtr<dynamicgraph::Vector, int>*
forcesSIN_[4];
155 typedef boost::mt11213b
ENG;
156 typedef boost::normal_distribution<double>
DIST;
157 typedef boost::variate_generator<ENG, DIST>
GEN;
double m_numericalDamping
constraint Jacobian
dynamicgraph::Signal< dynamicgraph::Vector, int > jointsAccelerationsSOUT_
joints' accelerations computed by the integrator
dynamicgraph::Vector temp6_
dynamicgraph::Signal< dynamicgraph::Vector, int > gyrometerSOUT_
Rotation velocity measured by gyrometers.
double timestep_
Current integration step.
dynamicgraph::Vector jointsVelocities_
base 6d pose + joints' angles
boost::normal_distribution< double > DIST
bool m_isTorqueControlled
DeviceTorqueCtrl(std::string RobotName)
Eigen::MatrixXd m_K
cholesky decomposition of the K matrix
tsid::math::Vector m_q_sot
static const double FORCE_SENSOR_NOISE_STD_DEV
RobotUtilShrPtr m_robot_util
dynamicgraph::Vector jointsAccelerations_
joints' velocities
dynamicgraph::Signal< dynamicgraph::Vector, int > accelerometerSOUT_
Accelerations measured by accelerometers.
Vector m_dv_c
Cholesky decomposition of _ZMZ.
virtual void init(const double &dt, const std::string &urdfFile)
tsid::math::Vector m_v_sot
static const std::string CLASS_NAME
static const double TIMESTEP_DEFAULT
boost::mt11213b ENG
number of joints
virtual void setVelocity(const dynamicgraph::Vector &vel)
dynamicgraph::Signal< dynamicgraph::Vector, int > p_gainsSOUT_
proportional and derivative position-control gains
dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * forcesSIN_[4]
input force sensor values
virtual ~DeviceTorqueCtrl()
dynamicgraph::Vector base6d_encoders_
dynamicgraph::Signal< dynamicgraph::Vector, int > jointsVelocitiesSOUT_
joints' velocities computed by the integrator
Eigen::LDLT< Eigen::MatrixXd > Cholesky
dynamicgraph::Vector gyrometer_
dynamicgraph::Signal< dynamicgraph::Vector, int > robotStateSOUT_
base 6d pose + joints' angles measured by encoders
virtual void integrate(const double &dt)
virtual void setStateSize(const unsigned int &size)
virtual const std::string & getClassName() const
dynamicgraph::Signal< dynamicgraph::Vector, int > currentSOUT_
motor currents
tsid::tasks::TaskSE3Equality * m_contactLF
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
boost::variate_generator< ENG, DIST > GEN
ENG randomNumberGenerator_
void computeForwardDynamics()
tsid::math::Vector m_dv_sot
tsid::tasks::TaskSE3Equality * m_contactRF
Eigen::JacobiSVD< Matrix > m_Jc_svd
dynamicgraph::Vector wrenches_[4]
joints' accelerations
Matrix m_ZMZ
base of constraint null space
dynamicgraph::Signal< dynamicgraph::Vector, int > d_gainsSOUT_
dynamicgraph::Vector accelerometer_
Intermediate variables to avoid allocation during control.
Vector m_tau_np6
solution of Jc*dv=-dJc*v
virtual void setControlInputType(const std::string &cit)
Vector m_dJcv
svd of the constraint matrix
virtual void setState(const dynamicgraph::Vector &st)
GEN normalRandomNumberGenerator_
Eigen::LDLT< Matrix > m_ZMZ_chol
projected mass matrix: Z_c^T*M*Z_c
DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector)
tsid::robots::RobotWrapper * m_robot
robot geometric/inertial data
Vector m_dvBar
constrained accelerations