9 #include <pinocchio/algorithm/joint-configuration.hpp> 10 #include <tsid/math/constraint-base.hpp> 11 #include <tsid/math/utils.hpp> 13 #include <dynamic-graph/factory.h> 14 #include <dynamic-graph/all-commands.h> 15 #include <sot/core/debug.hh> 21 using namespace sot::torque_control;
28 const double DeviceTorqueCtrl::TIMESTEP_DEFAULT = 0.001;
29 const double DeviceTorqueCtrl::FORCE_SENSOR_NOISE_STD_DEV = 1e-10;
33 DeviceTorqueCtrl::DeviceTorqueCtrl(std::string RobotName)
34 :
dgsot::Device(RobotName),
35 timestep_(TIMESTEP_DEFAULT),
36 m_initSucceeded(false),
37 accelerometerSOUT_(
"DeviceTorqueCtrl(" + RobotName +
")::output(vector)::accelerometer"),
38 gyrometerSOUT_(
"DeviceTorqueCtrl(" + RobotName +
")::output(vector)::gyrometer"),
39 robotStateSOUT_(
"DeviceTorqueCtrl(" + RobotName +
")::output(vector)::robotState"),
40 jointsVelocitiesSOUT_(
"DeviceTorqueCtrl(" + RobotName +
")::output(vector)::jointsVelocities"),
41 jointsAccelerationsSOUT_(
"DeviceTorqueCtrl(" + RobotName +
")::output(vector)::jointsAccelerations"),
42 currentSOUT_(
"DeviceTorqueCtrl(" + RobotName +
")::output(vector)::currents"),
43 p_gainsSOUT_(
"DeviceTorqueCtrl(" + RobotName +
")::output(vector)::p_gains"),
44 d_gainsSOUT_(
"DeviceTorqueCtrl(" + RobotName +
")::output(vector)::d_gains"),
45 CONSTRUCT_SIGNAL_IN(kp_constraints,
dynamicgraph::Vector),
46 CONSTRUCT_SIGNAL_IN(kd_constraints,
dynamicgraph::Vector),
47 CONSTRUCT_SIGNAL_IN(rotor_inertias,
dynamicgraph::Vector),
51 m_isTorqueControlled(true),
52 m_numericalDamping(1e-8),
53 normalDistribution_(0.0, FORCE_SENSOR_NOISE_STD_DEV),
54 normalRandomNumberGenerator_(randomNumberGenerator_, normalDistribution_) {
55 forcesSIN_[0] =
new SignalPtr<dynamicgraph::Vector, int>(NULL,
"DeviceTorqueCtrl::input(vector6)::inputForceRLEG");
56 forcesSIN_[1] =
new SignalPtr<dynamicgraph::Vector, int>(NULL,
"DeviceTorqueCtrl::input(vector6)::inputForceLLEG");
57 forcesSIN_[2] =
new SignalPtr<dynamicgraph::Vector, int>(NULL,
"DeviceTorqueCtrl::input(vector6)::inputForceRARM");
58 forcesSIN_[3] =
new SignalPtr<dynamicgraph::Vector, int>(NULL,
"DeviceTorqueCtrl::input(vector6)::inputForceLARM");
62 <<
d_gainsSOUT_ << m_kp_constraintsSIN << m_kd_constraintsSIN
63 << m_rotor_inertiasSIN << m_gear_ratiosSIN);
67 controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION;
70 dynamicgraph::Vector data(3);
77 for (
int i = 0; i < 4; ++i) {
78 withForceSignals[i] =
true;
81 if (i == FORCE_SIGNAL_RLEG || i == FORCE_SIGNAL_LLEG)
wrenches_[i](2) = 350.0;
89 std::string docstring;
92 "\n Integrate dynamics for time step provided as input\n" 93 "\n take one floating point number as input\n\n";
94 addCommand(
"increment", makeCommandVoid1((Device&)*
this, &Device::increment, docstring));
96 docCommandVoid2(
"Initialize the entity.",
"Time period in seconds (double)",
97 "Robot reference (string)")));
103 if (dt <= 0.0)
return SEND_MSG(
"Init failed: Timestep must be positive", MSG_TYPE_ERROR);
106 std::string localName(robotRef);
107 if (isNameInRobotUtil(localName))
110 SEND_MSG(
"You should first initialize the entity control-manager", MSG_TYPE_ERROR);
116 const dynamicgraph::sot::Vector6d& kp_contact = m_kp_constraintsSIN(0);
117 const dynamicgraph::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0);
118 const Vector rotor_inertias = m_rotor_inertiasSIN(0);
119 const Vector gear_ratios = m_gear_ratiosSIN(0);
120 const std::string& urdfFile =
m_robot_util->m_urdf_filename;
123 vector<string> package_dirs;
124 m_robot =
new robots::RobotWrapper(urdfFile, package_dirs, pinocchio::JointModelFreeFlyer());
126 m_robot->rotor_inertias(rotor_inertias);
127 m_robot->gear_ratios(gear_ratios);
152 }
catch (
const std::exception& e) {
153 std::cout << e.what();
154 return SEND_MSG(
"Init failed: Could load URDF :" + urdfFile, MSG_TYPE_ERROR);
162 assert(size == static_cast<unsigned int>(
m_nj + 6));
163 Device::setStateSize(size);
177 assert(q.size() ==
m_nj + 6);
179 SEND_WARNING_STREAM_MSG(
"Cannot setState before initialization!");
187 pinocchio::SE3 H_lf =
189 tsid::trajectories::TrajectorySample s(12, 6);
190 tsid::math::SE3ToVector(H_lf, s.pos);
192 SEND_MSG(
"Setting left foot reference to " + toString(H_lf), MSG_TYPE_DEBUG);
194 pinocchio::SE3 H_rf =
196 tsid::math::SE3ToVector(H_rf, s.pos);
198 SEND_MSG(
"Setting right foot reference to " + toString(H_rf), MSG_TYPE_DEBUG);
203 assert(v.size() ==
m_nj + 6);
205 SEND_WARNING_STREAM_MSG(
"Cannot setVelocity before initialization!");
208 Device::setVelocity(v);
214 if (cit ==
"torque") {
219 return Device::setControlInputType(cit);
223 const Vector& tauDes = controlSIN.accessCopy();
224 assert(tauDes.size() ==
m_nj);
231 assert(constrRF.matrix().rows() == 6 && constrRF.matrix().cols() ==
m_nj + 6);
232 assert(constrLF.matrix().rows() == 6 && constrLF.matrix().cols() ==
m_nj + 6);
233 assert(constrRF.vector().size() == 6 && constrLF.vector().size() == 6);
235 assert(
m_K.rows() ==
m_nj + 6 + 12 &&
m_K.cols() ==
m_nj + 6 + 12);
236 m_Jc.topRows<6>() = constrRF.matrix();
237 m_Jc.bottomRows<6>() = constrLF.matrix();
238 Matrix JcT =
m_Jc.transpose();
239 m_dJcv.head<6>() = constrRF.vector();
240 m_dJcv.tail<6>() = constrLF.vector();
243 m_Jc_svd.compute(
m_Jc, Eigen::ComputeThinU | Eigen::ComputeFullV);
247 Eigen::VectorXd::Index r = (
m_Jc_svd.singularValues().array() > 1e-8).count();
262 SEND_MSG(
"ZMZ*dv - ZT*(tau-h-Mdv_bar) = " + toString((
m_ZMZ *
m_dv_c - rhs).norm()), MSG_TYPE_DEBUG);
284 if ((JcT *
m_f - b).norm() > 1e-10)
285 SEND_MSG(
"M*dv +h - JT*f - tau = " + toString((JcT *
m_f - b).norm()), MSG_TYPE_DEBUG);
287 SEND_MSG(
"Jc*dv - dJc*v = " + toString((
m_Jc *
m_dv -
m_dJcv).norm()), MSG_TYPE_DEBUG);
292 SEND_WARNING_STREAM_MSG(
"Cannot integrate before initialization!");
310 Device::integrate(dt);
311 if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION)
326 for (
int i = 0; i < 4; i++) {
333 forcesSOUT[i]->setConstant(
temp6_);
341 for (
int i = 0; i < 4; ++i) forcesSOUT[i]->setTime(time);
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
bool m_isTorqueControlled
Eigen::MatrixXd m_K
cholesky decomposition of the K matrix
tsid::math::Vector m_q_sot
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
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
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)
VectorXd svdSolveWithDamping(const JacobiSVD< MatrixXd > &A, const VectorXd &b, double damping)
dynamicgraph::Signal< dynamicgraph::Vector, int > currentSOUT_
motor currents
tsid::tasks::TaskSE3Equality * m_contactLF
void computeForwardDynamics()
tsid::math::Vector m_dv_sot
tsid::tasks::TaskSE3Equality * m_contactRF
Eigen::JacobiSVD< Matrix > m_Jc_svd
numerical damping to regularize constraint resolution
dynamicgraph::Vector wrenches_[4]
joints' accelerations
tsid::math::ConstraintBase ConstraintBase
Matrix m_ZMZ
base of constraint null space
dynamicgraph::Signal< dynamicgraph::Vector, int > d_gainsSOUT_
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_
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DeviceTorqueCtrl, "DeviceTorqueCtrl")
tsid::robots::RobotWrapper * m_robot
robot geometric/inertial data
Vector m_dvBar
constrained accelerations