iCub-main
optimalControl.h
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
4  * All rights reserved.
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 
60 #ifndef __OPTIMALCONTROL_H__
61 #define __OPTIMALCONTROL_H__
62 
63 #include <yarp/sig/Vector.h>
64 #include <yarp/sig/Matrix.h>
65 #include <iCub/ctrl/math.h>
66 
67 
68 namespace iCub
69 {
70 
71 namespace ctrl
72 {
73 
79 class Riccati
80 {
81 protected:
82  yarp::sig::Matrix A, At;
83  yarp::sig::Matrix B, Bt;
84  yarp::sig::Matrix V, VN;
85  yarp::sig::Matrix P;
86 
87  yarp::sig::Matrix TN, lastT;
88  yarp::sig::Matrix *Ti;
89  yarp::sig::Matrix *Li;
90 
91  yarp::sig::Vector x;
92 
93  size_t n;
94  size_t m;
95  int N;
96 
97  bool verbose;
98 
99 public:
109  Riccati(const yarp::sig::Matrix &_A, const yarp::sig::Matrix &_B,
110  const yarp::sig::Matrix &_V, const yarp::sig::Matrix &_P,
111  const yarp::sig::Matrix &_VN, bool verb=false);
112 
118  yarp::sig::Matrix L(int step);
119 
125  yarp::sig::Matrix T(int step);
126 
136  void setProblemData(const yarp::sig::Matrix &_A, const yarp::sig::Matrix &_B,
137  const yarp::sig::Matrix &_V, const yarp::sig::Matrix &_P,
138  const yarp::sig::Matrix &_VN);
139 
147  void solveRiccati(int steps);
148 
155  yarp::sig::Vector doLQcontrol(int step, const yarp::sig::Vector &x);
156 
164  void doLQcontrol(int step, const yarp::sig::Vector &x, yarp::sig::Vector &ret);
165 
171  void setVerbose(bool verb=true);
172 };
173 
174 }
175 
176 }
177 
178 
179 #endif
180 
181 
Classic Riccati recursive formula for optimal control in a LQ problem.
yarp::sig::Matrix * Li
yarp::sig::Matrix * Ti
yarp::sig::Vector x
yarp::sig::Matrix A
Riccati(const yarp::sig::Matrix &_A, const yarp::sig::Matrix &_B, const yarp::sig::Matrix &_V, const yarp::sig::Matrix &_P, const yarp::sig::Matrix &_VN, bool verb=false)
Constructor, with initialization of algebraic Riccati equation.
yarp::sig::Matrix At
yarp::sig::Matrix VN
yarp::sig::Matrix Bt
yarp::sig::Matrix B
void setVerbose(bool verb=true)
Enable or disable verbose feedback (that is, printing additional information)
yarp::sig::Vector doLQcontrol(int step, const yarp::sig::Vector &x)
Compute the LQ feedback control, in the form: ret= - L(i) * x.
void solveRiccati(int steps)
Solve recursively discrete algebraic Riccati equation (DARE) and stores matrices Ti and Li,...
void setProblemData(const yarp::sig::Matrix &_A, const yarp::sig::Matrix &_B, const yarp::sig::Matrix &_V, const yarp::sig::Matrix &_P, const yarp::sig::Matrix &_VN)
Initialization of algebraic Riccati equation.
yarp::sig::Matrix TN
yarp::sig::Matrix lastT
yarp::sig::Matrix V
yarp::sig::Matrix P
yarp::sig::Matrix L(int step)
Get stored L_i matrix; call this function only after solveRiccati()
yarp::sig::Matrix T(int step)
Get stored T_i matrix; call this function only after solveRiccati()
void doLQcontrol(int step, const yarp::sig::Vector &x, yarp::sig::Vector &ret)
Compute the LQ feedback control, in the form: u= - L(i) * x.
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.