|
A sparse MPC solver for walking motion generation (old version).
|
00001 00009 /**************************************** 00010 * INCLUDES 00011 ****************************************/ 00012 00013 #include "rect_constraint.h" 00014 00015 /**************************************** 00016 * FUNCTIONS 00017 ****************************************/ 00018 00041 RectangularConstraint_ZMP::RectangularConstraint_ZMP(const double *d_) : vert(4,2) 00042 { 00043 D[0] = 1.0; D[4] = 0.0; 00044 D[1] = 0.0; D[5] = 1.0; 00045 D[2] = -1.0; D[6] = 0.0; 00046 D[3] = 0.0; D[7] = -1.0; 00047 00048 d[0] = d_[0]; 00049 d[1] = d_[1]; 00050 d[2] = d_[2]; 00051 d[3] = d_[3]; 00052 00053 d_orig[0] = d[0]; 00054 d_orig[1] = d[1]; 00055 d_orig[2] = d[2]; 00056 d_orig[3] = d[3]; 00057 00058 Constraints2Vert(); // determine coordinates of vertices 00059 } 00060 00061 00062 00074 void RectangularConstraint_ZMP::rotate_translate(const double ca, const double sa, const double x, const double y) 00075 { 00076 // D = D*R' 00077 D[0] = ca; D[4] = sa; 00078 D[1] = -sa; D[5] = ca; 00079 D[2] = -ca; D[6] = -sa; 00080 D[3] = sa; D[7] = -ca; 00081 00082 // d = d - D*p 00083 d[0] = d_orig[0] + D[0]*x + D[4]*y; 00084 d[1] = d_orig[1] + D[1]*x + D[5]*y; 00085 d[2] = d_orig[2] + D[2]*x + D[6]*y; 00086 d[3] = d_orig[3] + D[3]*x + D[7]*y; 00087 00088 Constraints2Vert(); // determine coordinates of vertices 00089 } 00090 00091 00092 00103 void RectangularConstraint_ZMP::Constraints2Vert() 00104 { 00105 double det; 00106 00107 // Indexes of ctr.D 00108 // 0 4 00109 // 1 5 00110 // 2 6 00111 // 3 7 00112 00113 // D d inv (D) 00114 // |0 4| 0 1/det * | 7 -4| 00115 // |3 7| 3 |-3 0| 00116 det = D[0]*D[7] - D[3]*D[4]; 00117 vert(0,0) = D[7]/det*d[0] - D[4]/det*d[3]; 00118 vert(0,1) = -D[3]/det*d[0] + D[0]/det*d[3]; 00119 00120 // |0 4| 0 | 5 -4| 00121 // |1 5| 1 |-1 0| 00122 det = D[0]*D[5] - D[4]*D[1]; 00123 vert(1,0) = D[5]/det*d[0] - D[4]/det*d[1]; 00124 vert(1,1) = -D[1]/det*d[0] + D[0]/det*d[1]; 00125 00126 // |1 5| 1 | 6 -5| 00127 // |2 6| 2 |-2 1| 00128 det = D[1]*D[6] - D[5]*D[2]; 00129 vert(2,0) = D[6]/det*d[1] - D[5]/det*d[2]; 00130 vert(2,1) = -D[2]/det*d[1] + D[1]/det*d[2]; 00131 00132 // |2 6| 2 | 7 -6| 00133 // |3 7| 3 |-3 2| 00134 det = D[2]*D[7] - D[3]*D[6]; 00135 vert(3,0) = D[7]/det*d[2] - D[6]/det*d[3]; 00136 vert(3,1) = -D[3]/det*d[2] + D[2]/det*d[3]; 00137 } 00138
1.8.0