iCub-main
optimalControl.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10
11 #include <cmath>
12
13 #include <yarp/os/Log.h>
14 #include <yarp/math/Math.h>
15 #include <yarp/math/SVD.h>
17
18 using namespace std;
19 using namespace yarp::os;
20 using namespace yarp::sig;
21 using namespace yarp::math;
22 using namespace iCub::ctrl;
23
24
25 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
26 Riccati::Riccati(const Matrix &_A, const Matrix &_B, const Matrix &_V,
27  const Matrix &_P, const Matrix &_VN, bool verb)
28 {
29  A = _A; At = A.transposed();
30  B = _B; Bt = B.transposed();
31  V = _V;
32  P = _P;
33  VN = _VN;
34
35  Ti = new Matrix[1]; Ti[0].resize(1,1); Ti[0].zero();
36  Li = new Matrix[1]; Li[0].resize(1,1); Li[0].zero();
37
38  n=A.rows();
39  m=B.rows();
40  N=-1;
41
42  verbose=verb;
43  if (verbose)
44  yWarning("Riccati: problem defined, unsolved.");
45 }
46
47
48 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
49 void Riccati::setVerbose(bool verb)
50 {
51  verbose=verb;
52 }
53
54
55 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
56 Matrix Riccati::L(int step)
57 {
58  if(N<0)
59  {
60  if (verbose)
61  yWarning("Riccati: DARE has not been solved yet.");
62  return Li[0];
63  }
64  if(step>=0 && step>=N)
65  {
66  if (verbose)
67  yWarning("Riccati: Index for gain matrix out of bound.");
68  return Li[0];
69  }
70  return Li[step];
71 }
72
73
74 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
75 Matrix Riccati::T(int step)
76 {
77  if(N<0)
78  {
79  if (verbose)
80  yError("Riccati: DARE has not been solved yet.");
81  return Ti[0];
82  }
83  if(step>=0 && step>N)
84  {
85  if (verbose)
86  yError("Riccati: Index for DARE matrix out of bound.");
87  return Ti[0];
88  }
89  return Ti[step];
90 }
91
92
93 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
94 void Riccati::setProblemData(const Matrix &_A, const Matrix &_B, const Matrix &_V,
95  const Matrix &_P, const Matrix &_VN)
96 {
97  A = _A; At = A.transposed();
98  B = _B; Bt = B.transposed();
99  V = _V;
100  P = _P;
101  VN = _VN;
102
103  n=A.rows();
104  m=B.rows();
105  N=-1;
106
107  if (verbose)
108  yWarning("Riccati: problem defined, unsolved.");
109 }
110
111
112 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
113 void Riccati::solveRiccati(int steps)
114 {
115  int i;
116  N = steps;
117  delete [] Ti;
118  delete [] Li;
119  Ti = new Matrix[steps+1];
120  Li = new Matrix[steps];
121  for(i=0; i<=steps; i++)
122  Ti[i].resize(VN.rows(),VN.cols());
123  //init TN=VN
124  Ti[steps]=VN;
125  //compute backward all Ti
126  for(i=steps-1; i>=0; i--)
127  {
128  lastT=Ti[i+1];
129  //Ti = V + A' * (Ti+1 - Ti+1 * B * (P + B' * Ti+1 * B)^-1 * B' * Ti+1 )* A;
130  Ti[i] = V + At *(lastT - lastT * B* pinv(P+Bt*lastT*B)*Bt*lastT )* A;
131  }
132  //compute forward all Li
133  for(i=0;i<steps; i++)
134  {
135  //Li = (P + B' * Ti+1 * B)^-1 * B' * Ti+1 * A
136  Li[i] = pinv(P + Bt*Ti[i+1]*B) *Bt * Ti[i+1] * A;
137  }
138
139  if (verbose)
140  yInfo("Riccati: DARE solved, matrices Li and Ti computed and stored.");
141 }
142
143
144 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
145 Vector Riccati::doLQcontrol(int step, const Vector &x)
146 {
147  if(N<0)
148  {
149  if (verbose)
150  yError("Riccati: DARE has not been solved yet.");
151  Vector ret(1,0.0);
152  return ret;
153  }
154  if(step>=0 && step>N)
155  {
156  if (verbose)
157  yError("Riccati: Index for DARE matrix out of bound.");
158  Vector ret(1,0.0);
159  return ret;
160  }
161  return (Li[step] * (-1.0*x));
162 }
163
164
165 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
166 void Riccati::doLQcontrol(int step, const Vector &x, Vector &ret)
167 {
168  if(N<0)
169  {
170  if (verbose)
171  yError("Riccati: DARE has not been solved yet.");
172  ret.zero();
173  }
174  else if(step>=0 && step>N)
175  {
176  if (verbose)
177  yError("Riccati: Index for DARE matrix out of bound.");
178  ret.zero();
179  }
180  else
181  {
182  ret.resize(m);
183  ret = Li[step] * (-1.0*x);
184  }
185 }
186
187
int n
A
Definition: sine.m:16