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;
23 using namespace tsid::tasks;
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);