Jaesung Oh
quadprog++ and Eigen library test
Dependencies: mbed Eigen FastPWM
Diff: main.cpp
- Revision:
- 2:e843c1b0b25c
- Parent:
- 1:27f1640d930d
diff -r 27f1640d930d -r e843c1b0b25c main.cpp
--- a/main.cpp Wed Aug 21 05:18:06 2019 +0000
+++ b/main.cpp Tue Sep 24 00:20:12 2019 +0000
@@ -3,62 +3,345 @@
#define PIN_A PA_8 // channel 1
#define PIN_B PA_7 // complementary
-#define PWM_ARR 0x8CA /// timer autoreload value 40k loop, 40k pwm
+#define PWM_ARR 4500 /// timer autoreload value 40k loop, 40k pwm
+
+#include "quadprog.h"
+#include <cstdlib>
+#include "Eigen/Dense.h"
+
+using namespace std;
+using namespace Eigen;
+
+MatrixXd AA = MatrixXd::Zero(6,1);
+
+class JS_QP
+{
+private:
+ int NUMCOLS;//length of X
+ int NUMEQ;
+ int NUMINEQ;
+
+ MatrixXd A_ineq,B_ineq;
+ MatrixXd A_eq,B_eq;
+
+public:
+ MatrixXd H,F;
+ MatrixXd X;
+
+public:
+ void setNums(int _xlength, int numEqConstraints, int numIneqConstraints)
+ {
+ NUMCOLS = _xlength;//should be 18+12+3*contact
+ NUMEQ = numEqConstraints;//NUMCOLS(dynamics) + NUMCOLS(contactconstraints + swingleg)
+ //contact to be weight?
+ NUMINEQ = numIneqConstraints;//friction cone approximation -ufz<fx<ufz, -ufz<fy<ufz ->4
+
+ //maybe matrix assignment here
+ X = MatrixXd::Zero(NUMCOLS,1);
+ A_eq = MatrixXd::Zero(NUMEQ, NUMCOLS);
+ B_eq = MatrixXd::Zero(NUMEQ, 1);
+
+ A_ineq = MatrixXd::Zero(NUMINEQ, NUMCOLS);
+ B_ineq = MatrixXd::Zero(NUMINEQ, 1);
+
+
+ H = MatrixXd::Zero(NUMCOLS,NUMCOLS);
+ F = MatrixXd::Zero(NUMCOLS,1);
+ }
+
+ void make_EQ(MatrixXd A, MatrixXd b)
+ {
+ //Aeq*X = Beq
+ A_eq = A;
+ B_eq = b;
+ }
+
+ void make_IEQ(MatrixXd A, MatrixXd b)
+ {
+ // Aineq*X < Bineq
+ A_ineq = A;
+ B_ineq = b;
+ }
+
+ void make_HF(MatrixXd A, MatrixXd b, int mode=-1)
+ {
+ //min (0.5* x H x + F x)
+ if(mode == -1) {
+ H = A.transpose() * A + MatrixXd::Identity(NUMCOLS,NUMCOLS)*1e-3;
+ F = -A.transpose() * b;
-float dtc_a=0.1;
-float dtc_b=0.2;
-void Init_PWM();
-DigitalOut check(PC_9);
-DigitalIn a1(PB_7);
-DigitalIn a2(PB_6);
+ } else {
+ H = A;
+ F = b;
+
+ }
+
+ }
+
+ MatrixXd solve_QP()
+ {
+ quadprogpp::Vector<double> outX;
+ quadprogpp::Matrix<double> G;
+ quadprogpp::Vector<double> g0;
+ quadprogpp::Matrix<double> CE;
+ quadprogpp::Vector<double> ce0;
+ quadprogpp::Matrix<double> CI;
+ quadprogpp::Vector<double> ci0;
+ //min 0.5 * x G x + g0 x
+ //CE^T x + ce0 = 0
+ //CI^T x + ci0 >= 0
+
+ G.resize(NUMCOLS,NUMCOLS);
+ g0.resize(NUMCOLS);
+ for(int i=0; i<NUMCOLS; i++) {
+ for(int j=0; j<NUMCOLS; j++) {
+ G[i][j] = H(i,j);
+ }
+ g0[i] = F(i,0);
+ }
+ CE.resize(NUMCOLS,NUMEQ);
+ ce0.resize(NUMEQ);
+ for(int i=0; i<NUMCOLS; i++) {
+ for(int j=0; j<NUMEQ; j++) {
+ CE[i][j] = -A_eq(j,i);
+ }
+ }
+ for(int j=0; j<NUMEQ; j++) {
+ ce0[j] = B_eq(j,0);
+ }
+ CI.resize(NUMCOLS,NUMINEQ);
+ ci0.resize(NUMINEQ);
+ for(int i=0; i<NUMCOLS; i++) {
+ for(int j=0; j<NUMINEQ; j++) {
+ CI[i][j] = -A_ineq(j,i);
+ }
+ }
+ for(int j=0; j<NUMINEQ; j++) {
+ ci0[j] = B_ineq(j,0);
+ }
+ outX.resize(NUMCOLS);
+
+ solve_quadprog(G, g0, CE, ce0, CI, ci0, outX);
+ for(int i=0; i<NUMCOLS; i++) {
+ X(i,0) = outX[i];
+ }
+
+ return X;
+
+ }
+
+public:
+
+};
+
+JS_QP js_qp;
+
+//MatrixXd AA;
-extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
- if (TIM1->SR & TIM_SR_UIF ) {
- check=1;
- TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_a); // Write duty cycles
- check=0;
+float dtc_a=0;
+void Init_PWM();
+
+AnalogIn Vfb(PA_5);
+
+extern "C" void TIM1_UP_TIM10_IRQHandler(void)
+{
+ if (TIM1->SR & TIM_SR_UIF ) {
+
+ float Vth = 0.51f;
+
+ if(Vfb >= Vth) { //PA_8 이게 하이
+ dtc_a -= 0.001f;
+ }
+
+ else {
+ dtc_a += 0.001f;
+ }
+
+ if(dtc_a>=0.9f)
+ dtc_a = 0.9f;
+
+ if(dtc_a <= 0.0f)
+ dtc_a = 0.0f;
+
+ dtc_a = 0.5f;
+
+ TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_a); // Write duty cycles
+
}
-TIM1->SR = 0x0; // reset the status register
+ TIM1->SR = 0x0; // reset the status register
}
+Timer t;
-int main(){
- check=1;
- Init_PWM();
- TIM1->CR1 ^= TIM_CR1_UDIS;
- while(1);
+double t_old=0;
+
+int main()
+{
+
+ MatrixXd Aa = MatrixXd::Zero(20,2);
+ MatrixXd Ba = MatrixXd::Zero(20,10);
+ MatrixXd Ga = MatrixXd::Zero(20,20);
+
+ Aa <<
+ 1.000000,-1.000000
+ ,0.000000,0.783386
+ ,1.000000,-1.783386
+ ,0.000000,0.613693
+ ,1.000000,-2.397079
+ ,0.000000,0.480759
+ ,1.000000,-2.877838
+ ,0.000000,0.376620
+ ,1.000000,-3.254458
+ ,0.000000,0.295039
+ ,1.000000,-3.549496
+ ,0.000000,0.231129
+ ,1.000000,-3.780626
+ ,0.000000,0.181063
+ ,1.000000,-3.961689
+ ,0.000000,0.141842
+ ,1.000000,-4.103531
+ ,0.000000,0.111117
+ ,1.000000,-4.214648
+ ,0.000000,0.087048;
+
+
+ Ba <<
+ 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000
+ ,-1.674291,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000
+ ,0.109023,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000
+ ,-1.783314,-1.674291,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000
+ ,0.085407,0.109023,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000
+ ,-1.868721,-1.783314,-1.674291,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000
+ ,0.066907,0.085407,0.109023,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000
+ ,-1.935628,-1.868721,-1.783314,-1.674291,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000
+ ,0.052414,0.066907,0.085407,0.109023,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698;
+
+
+ Ga <<
+ 1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.570138,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,-0.074613,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.644751,1.000000,0.570138,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,-0.058451,0.000000,-0.074613,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.703202,1.000000,0.644751,1.000000,0.570138,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,-0.045790,0.000000,-0.058451,0.000000,-0.074613,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.748992,1.000000,0.703202,1.000000,0.644751,1.000000,0.570138,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+ ,0.000000,-0.035871,0.000000,-0.045790,0.000000,-0.058451,0.000000,-0.074613,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000
+ ,1.000000,0.784863,1.000000,0.748992,1.000000,0.703202,1.000000,0.644751,1.000000,0.570138,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000
+ ,0.000000,-0.028101,0.000000,-0.035871,0.000000,-0.045790,0.000000,-0.058451,0.000000,-0.074613,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000
+ ,1.000000,0.812964,1.000000,0.784863,1.000000,0.748992,1.000000,0.703202,1.000000,0.644751,1.000000,0.570138,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000
+ ,0.000000,-0.022014,0.000000,-0.028101,0.000000,-0.035871,0.000000,-0.045790,0.000000,-0.058451,0.000000,-0.074613,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113;
+
+ MatrixXd ref_a = MatrixXd::Zero(20,1);
+ MatrixXd w0_a = MatrixXd::Zero(20,1);
+
+ MatrixXd At;
+ MatrixXd Bt;
+
+ MatrixXd X_state = MatrixXd::Zero(2,1);
+
+ Bt = ref_a - Aa*X_state - Ga*w0_a;
+ At = Ba;
+
+ MatrixXd Q = MatrixXd::Identity(20,20);
+ MatrixXd R = MatrixXd::Identity(10,10);
+
+ MatrixXd H;
+ MatrixXd F;
+
+ H = At.transpose() * Q * At + R;
+ F = -At.transpose()*Bt;
+
+
+ MatrixXd out;
+
+ MatrixXd Aieq = MatrixXd::Zero(20,10);
+ MatrixXd bieq = MatrixXd::Zero(20,1);
+
+ Aieq.block(0,0,10,10) = MatrixXd::Identity(10,10);
+ Aieq.block(10,0,10,10) = -MatrixXd::Identity(10,10);
+
+ for(int i=0; i<10; i++) {
+ bieq(i,0) = 10;
+ bieq(i+10,0) = 10;
+ }
+
+
+ while(1) {
+
+ js_qp.setNums(10,0,20);
+
+ js_qp.make_HF(H,F,1);
+ js_qp.make_IEQ(Aieq,bieq);
+
+ t.reset();
+ t.start();
+
+ out = js_qp.solve_QP();
+
+ t.stop();
+
+
+ cout << "out : " << endl;
+ cout << out << endl;
+ cout << "time : " << endl;
+ cout << t.read() << endl;
+
+
+ wait(1);
+
+ }
}
-void Init_PWM(){
+void Init_PWM()
+{
RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock
- FastPWM pwm_a(PIN_A); // 단순히 핀 설정용
+ FastPWM pwm_a(PIN_A); // 단순히 핀 설정용
FastPWM pwm_b(PIN_B);
-
- //ISR Setup
-
+
+ //ISR Setup
+
NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ
TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
TIM1->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode
TIM1->CR1 |= TIM_CR1_UDIS;
- TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on,
- TIM1->RCR |= 0x001; // update event once per up/down count of tim1
+ TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on,
+ TIM1->RCR |= 0x001; // update event once per up/down count of tim1
TIM1->EGR |= TIM_EGR_UG;
-
+
//PWM Setup
- TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
+ TIM1->PSC = 0; // no prescaler, timer counts up in sync with the peripheral clock
TIM1->ARR = PWM_ARR; // set auto reload, 40 khz
// TIM1->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on.
TIM1->CR1 |= TIM_CR1_CEN; // enable TIM1
-
- // for complementary
+
+ // for complementary
TIM1->CCER|=0b1111;
- }
\ No newline at end of file
+}
\ No newline at end of file