sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
ddp_pyrene_actuator_solver.cpp
Go to the documentation of this file.
1/*
2 * Copyright 2018-, Olivier Stasse LAAS-CNRS
3 *
4 * This file is part of sot-torque-control.
5 * sot-torque-control is free software: you can redistribute it and/or
6 * modify it under the terms of the GNU Lesser General Public License
7 * as published by the Free Software Foundation, either version 3 of
8 * the License, or (at your option) any later version.
9 * sot-torque-control is distributed in the hope that it will be
10 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU Lesser General Public License for more details. You should
13 * have received a copy of the GNU Lesser General Public License along
14 * with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
15 */
16
17#define EIGEN_NO_MALLOC
18#include <dynamic-graph/factory.h>
19#include <math.h>
20
21#include <Eigen/Dense>
22#include <sot/core/debug.hh>
25
26#if DEBUG
27#define ODEBUG(x) std::cout << x << std::endl
28#else
29#define ODEBUG(x)
30#endif
31#define ODEBUG3(x) std::cout << x << std::endl
32
33#define DBGFILE "/tmp/debug-ddp_pyrene_actuator_solver.dat"
34
35#define RESETDEBUG5() \
36 { \
37 std::ofstream DebugFile; \
38 DebugFile.open(DBGFILE, std::ofstream::out); \
39 DebugFile.close(); \
40 }
41#define ODEBUG5FULL(x) \
42 { \
43 std::ofstream DebugFile; \
44 DebugFile.open(DBGFILE, std::ofstream::app); \
45 DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
46 << "):" << x << std::endl; \
47 DebugFile.close(); \
48 }
49#define ODEBUG5(x) \
50 { \
51 std::ofstream DebugFile; \
52 DebugFile.open(DBGFILE, std::ofstream::app); \
53 DebugFile << x << std::endl; \
54 DebugFile.close(); \
55 }
56
57#define RESETDEBUG4()
58#define ODEBUG4FULL(x)
59#define ODEBUG4(x)
60
61namespace dynamicgraph {
62namespace sot {
63namespace torque_control {
64
65namespace dynamicgraph = ::dynamicgraph;
66using namespace dynamicgraph;
67using namespace dynamicgraph::command;
68using namespace Eigen;
69
70#define ALL_INPUT_SIGNALS \
71 m_pos_desSIN << m_pos_joint_measureSIN << m_dx_joint_measureSIN \
72 << m_tau_desSIN
73
74#define ALL_OUTPUT_SIGNALS m_tauSOUT
75
78typedef DdpPyreneActuatorSolver EntityClassName;
79
80/* --- DG FACTORY ------------------------------------------------------- */
82 "DdpPyreneActuatorSolver");
83
85 : Entity(name),
86 CONSTRUCT_SIGNAL_IN(pos_des, dynamicgraph::Vector),
87 CONSTRUCT_SIGNAL_IN(pos_joint_measure, dynamicgraph::Vector),
88 CONSTRUCT_SIGNAL_IN(dx_joint_measure, dynamicgraph::Vector),
89 CONSTRUCT_SIGNAL_IN(tau_des, dynamicgraph::Vector),
90 CONSTRUCT_SIGNAL_OUT(tau, dynamicgraph::Vector, ALL_INPUT_SIGNALS),
91 m_dt(1e-3),
92 m_T(50),
93 m_stopCrit(1e-3),
94 m_iterMax(10),
95 m_solver(m_model, m_cost, DISABLE_FULLDDP, DISABLE_QPBOX) {
97 Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
98
99 m_zeroState.setZero();
100
101 /* Commands. */
102 addCommand(
103 "init",
104 makeCommandVoid4(
106 docCommandVoid4("Initialize the DDP solver.", "Control timestep [s].",
107 "Size of the preview window (in nb of samples)",
108 "Max. nb. of iterations", "Stopping criteria")));
109 addCommand("setTorqueLimit",
110 makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setTorqueLimit,
111 docCommandVoid1("Set the Torque limit.",
112 "Limit of the motor torque.")));
113 addCommand(
114 "setJointLimit",
115 makeCommandVoid2(*this, &DdpPyreneActuatorSolver::setJointLimit,
116 docCommandVoid2("Set the angular limits of the joint.",
117 "Upper limit", "Lower limit.")));
118 addCommand(
119 "setJointVelLimit",
120 makeCommandVoid2(
122 docCommandVoid2("Set the angular velocity limits of the joint.",
123 "Upper limit", "Lower limit.")));
124 addCommand("setLoadParam",
125 makeCommandVoid3(*this, &DdpPyreneActuatorSolver::setLoadParam,
126 docCommandVoid3("Setter of the Load parameters.",
127 "Mass of the load [g].",
128 "X coordinate of the Load",
129 "Y coordinate of the Load")));
130 addCommand("setLoadMass",
131 makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setLoadMass,
132 docCommandVoid1("Set the Load mass.",
133 "Mass of the load [g].")));
134 addCommand("removeLoad",
135 makeCommandVoid0(*this, &DdpPyreneActuatorSolver::removeLoad,
136 docCommandVoid0("Remove the Load.")));
137
138 addCommand(
139 "setCostGainState",
140 makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setCostGainState,
141 docCommandVoid1("Set the Gain of the state cost matrix.",
142 "Matrix of Gains.")));
143 addCommand("setCostGainCommand",
144 makeCommandVoid1(
146 docCommandVoid1("Set the Gain of the command cost matrix.",
147 "Matrix of Gains.")));
148 addCommand(
149 "setCostGainStateConstraint",
150 makeCommandVoid1(
152 docCommandVoid1("Set the Gain of the constraints on the state.",
153 "Matrix of Gains.")));
154 addCommand("setCostGainTorqueConstraint",
155 makeCommandVoid1(
157 docCommandVoid1("Set the Gain of the torque constraints.",
158 "Matrix of Gains.")));
159
160 m_initSucceeded = true;
161}
162
163/* --- COMMANDS ---------------------------------------------------------- */
164DEFINE_SIGNAL_OUT_FUNCTION(tau, dynamicgraph::Vector) {
165 if (!m_initSucceeded) {
166 SEND_WARNING_STREAM_MSG("Cannot compute signal tau before initialization!");
167 return s;
168 }
169
172 const dynamicgraph::Vector& pos_des = m_pos_desSIN(iter);
174 const dynamicgraph::Vector& pos_joint_measure = m_pos_joint_measureSIN(iter);
176 const dynamicgraph::Vector& dx_joint_measure = m_dx_joint_measureSIN(iter);
178 const dynamicgraph::Vector& tau_des = m_tau_desSIN(iter);
179
180 DDPSolver<double, 2, 1>::stateVec_t xinit, xDes;
181
182 xinit << pos_joint_measure(0), dx_joint_measure(0);
183
184 xDes << pos_des, 0.0;
185 // ODEBUG(xDes);
186
187 m_solver.initSolver(xinit, xDes);
188 // ODEBUG("FirstInitSolver");
189
191 m_solver.solveTrajectory();
192 // ODEBUG("Trajectory solved");
193
195 DDPSolver<double, 2, 1>::traj lastTraj;
196 lastTraj = m_solver.getLastSolvedTrajectory();
197 // ODEBUG("getLastSolvedTrajectory");
198
199 DDPSolver<double, 2, 1>::commandVecTab_t uList;
200 uList = lastTraj.uList;
201
202 s = m_previous_tau;
203 double tau_ddp = uList[0](0, 0);
204 if (!isnan(tau_ddp)) {
205 s[25] = tau_ddp;
206 }
207
208 m_previous_tau = s;
209 // ODEBUG(s);
210
211 return s;
212}
213
214void DdpPyreneActuatorSolver::param_init(const double& timestep, const int& T,
215 const int& nbItMax,
216 const double& stopCriteria) {
217 if (!m_pos_desSIN.isPlugged())
218 return SEND_MSG("Init failed: signal pos_des is not plugged",
219 MSG_TYPE_ERROR);
220 if (!m_pos_joint_measureSIN.isPlugged())
221 return SEND_MSG("Init failed: signal pos_joint_measure is not plugged",
222 MSG_TYPE_ERROR);
223 if (!m_dx_joint_measureSIN.isPlugged())
224 return SEND_MSG("Init failed: signal dx_joint_measure is not plugged",
225 MSG_TYPE_ERROR);
226
227 m_previous_tau.resize(32);
228 m_previous_tau.setZero();
229 m_T = T;
230 m_dt = timestep;
231 m_iterMax = nbItMax;
232 m_stopCrit = stopCriteria;
233 m_cost.setTauLimit(70.0);
234 m_cost.setJointLimit(0.0, -2.35619449019);
235 m_cost.setJointVelLimit(30.0, -30.0);
236 m_solver.FirstInitSolver(m_zeroState, m_zeroState, m_T, m_dt, m_iterMax,
237 m_stopCrit);
238}
239
241 m_cost.setTauLimit(tau);
242}
243
244void DdpPyreneActuatorSolver::setJointLimit(const double& upperLim,
245 const double& lowerLim) {
246 m_cost.setJointLimit(upperLim, lowerLim);
247}
248
250 const double& lowerLim) {
251 m_cost.setJointVelLimit(upperLim, lowerLim);
252}
253
255 const double& coordX,
256 const double& coordY) {
257 m_model.setLoadParam(mass, coordX, coordY);
258}
259
260void DdpPyreneActuatorSolver::setLoadMass(const double& mass) {
261 m_model.setLoadMass(mass);
262}
263
265
266void DdpPyreneActuatorSolver::setCostGainState(const dynamicgraph::Vector& Q) {
267 const CostFunction<double, 2, 1>::stateMat_t Q_new =
268 Eigen::Map<const CostFunction<double, 2, 1>::stateMat_t,
269 Eigen::Unaligned>(Q.data(), 2, 1);
270 m_cost.setCostGainState(Q_new);
271}
272
274 const dynamicgraph::Vector& W) {
275 const CostFunction<double, 2, 1>::stateMat_t W_new =
276 Eigen::Map<const CostFunction<double, 2, 1>::stateMat_t,
277 Eigen::Unaligned>(W.data(), 2, 1);
278 m_cost.setCostGainStateConstraint(W_new);
279}
280
282 const dynamicgraph::Vector& R) {
283 const CostFunction<double, 2, 1>::commandMat_t R_new =
284 Eigen::Map<const CostFunction<double, 2, 1>::commandMat_t,
285 Eigen::Unaligned>(R.data(), 1);
286 m_cost.setCostGainCommand(R_new);
287}
288
290 const dynamicgraph::Vector& P) {
291 const CostFunction<double, 2, 1>::commandMat_t P_new =
292 Eigen::Map<const CostFunction<double, 2, 1>::commandMat_t,
293 Eigen::Unaligned>(P.data(), 1);
294 m_cost.setCostGainTorqueConstraint(P_new);
295}
296
297void DdpPyreneActuatorSolver::display(std::ostream& os) const {
298 os << " T: " << m_T << " timestep: " << m_dt << " nbItMax: " << m_iterMax
299 << " stopCriteria: " << m_stopCrit << std::endl;
300}
301
302} // namespace torque_control
303} // namespace sot
304} // namespace dynamicgraph
void setLoadParam(const double &mass, const double &coordX, const double &coordY)
void setJointVelLimit(const double &upperLim, const double &lowerLim)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DdpPyreneActuatorSolver(const std::string &name)
void setJointLimit(const double &upperLim, const double &lowerLim)
void param_init(const double &timestep, const int &T, const int &nbItMax, const double &stopCriteria)
#define RESETDEBUG5()
#define ALL_OUTPUT_SIGNALS
#define ALL_INPUT_SIGNALS
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition: treeview.dox:22