61 d.kktMat(0, 0) = knot.R;
62 d.kktMat(0, 1) = knot.D.transpose();
63 d.kktMat(1, 0) = knot.D;
64 d.kktMat(1, 1).diagonal().array() = -mueq;
66 VectorRef kff = d.ff[0] = -knot.r;
67 VectorRef zff = d.ff[1] = -knot.d;
68 RowMatrixRef K = d.fb.
blockRow(0) = -knot.S.transpose();
69 RowMatrixRef Z = d.fb.
blockRow(1) = -knot.C;
75 RowMatrixRef Kth = d.ft.
blockRow(0) = -knot.Gu;
76 RowMatrixRef Zth = d.ft.
blockRow(1) = -knot.Gv;
79 Eigen::Transpose Ct = knot.C.transpose();
81 v.Pxx.noalias() = knot.Q + knot.S * K;
82 v.Pxx.noalias() += Ct * Z;
84 v.Pxt.noalias() = knot.Gx + K.transpose() * knot.Gu;
85 v.Pxt.noalias() += Z.transpose() * knot.Gv;
87 v.Ptt.noalias() = knot.Gth + knot.Gu.transpose() * Kth;
88 v.Ptt.noalias() += knot.Gv.transpose() * Zth;
90 v.px.noalias() = knot.q + knot.S * kff;
91 v.px.noalias() += Ct * zff;
93 v.pt.noalias() = knot.gamma + knot.Gu.transpose() * kff;
94 v.pt.noalias() += knot.Gv.transpose() * zff;
98 const value *vn,
Scalar mudyn,
101 d.kktMat(0, 0) = knot.R;
102 d.kktMat(1, 0) = knot.D;
103 d.kktMat(0, 1) = knot.D.transpose();
104 d.kktMat(1, 1).diagonal().setConstant(-mueq);
106 d.kktMat(2, 0) = knot.B;
107 d.kktMat(0, 2) = knot.B.transpose();
108 d.kktMat(2, 2).diagonal().setConstant(-mudyn);
109 d.kktMat(2, 3) = knot.E;
110 d.kktMat(3, 2) = knot.E.transpose();
112 d.kktMat(3, 3) = vn->Pxx;
119 VectorRef kff = d.ff[0] = -knot.r;
120 VectorRef zff = d.ff[1] = -knot.d;
121 VectorRef lff = d.ff[2] = -knot.f;
122 VectorRef yff = d.ff[3];
127 RowMatrixRef K = d.fb.
blockRow(0) = -knot.S.transpose();
128 RowMatrixRef Z = d.fb.
blockRow(1) = -knot.C;
129 RowMatrixRef L = d.fb.
blockRow(2) = -knot.A;
130 RowMatrixRef Y = d.fb.
blockRow(3).setZero();
133 RowMatrixRef Kth = d.ft.
blockRow(0) = -knot.Gu;
134 RowMatrixRef Zth = d.ft.
blockRow(1) = -knot.Gv;
136 RowMatrixRef Yth = d.ft.
blockRow(3);
145 Eigen::Transpose At = knot.A.transpose();
146 Eigen::Transpose Ct = knot.C.transpose();
148 v.Pxx.noalias() = knot.Q + knot.S * K;
149 v.Pxx.noalias() += Ct * Z;
150 v.Pxx.noalias() += At * L;
153 v.Pxt.noalias() += K.transpose() * knot.Gu;
154 v.Pxt.noalias() += Z.transpose() * knot.Gv;
156 v.Pxt.noalias() += Y.transpose() * vn->Pxt;
159 v.Ptt.noalias() += Kth.transpose() * knot.Gu;
160 v.Ptt.noalias() += Zth.transpose() * knot.Gv;
162 v.Ptt.noalias() += Yth.transpose() * vn->Pxt;
164 v.px.noalias() = knot.q + knot.S * kff;
165 v.px.noalias() += Ct * zff;
166 v.px.noalias() += At * lff;
168 v.pt.noalias() = knot.gamma + knot.Gu.transpose() * kff;
169 v.pt.noalias() += knot.Gv.transpose() * zff;
171 v.pt.noalias() += vn->Pxt.transpose() * yff;
175 const Data &d, boost::span<VectorXs> xs,
176 boost::span<VectorXs> us, boost::span<VectorXs> vs,
177 boost::span<VectorXs> lbdas,
178 const std::optional<ConstVectorRef> &theta_) {
179 ConstVectorRef kff = d.ff[0];
180 ConstVectorRef zff = d.ff[1];
181 ConstVectorRef lff = d.ff[2];
182 ConstVectorRef yff = d.ff[3];
184 ConstRowMatrixRef K = d.fb.
blockRow(0);
185 ConstRowMatrixRef Z = d.fb.
blockRow(1);
186 ConstRowMatrixRef L = d.fb.
blockRow(2);
187 ConstRowMatrixRef Y = d.fb.
blockRow(3);
189 ConstRowMatrixRef Kth = d.ft.
blockRow(0);
190 ConstRowMatrixRef Zth = d.ft.
blockRow(1);
191 ConstRowMatrixRef Lth = d.ft.
blockRow(2);
192 ConstRowMatrixRef Yth = d.ft.
blockRow(3);
195 us[i].noalias() = kff + K * xs[i];
196 vs[i].noalias() = zff + Z * xs[i];
197 if (theta_.has_value()) {
199 us[i].noalias() += Kth * theta_.value();
200 vs[i].noalias() += Zth * theta_.value();
205 lbdas[i + 1].noalias() = lff + L * xs[i];
206 xs[i + 1].noalias() = yff + Y * xs[i];
207 if (theta_.has_value()) {
208 lbdas[i + 1].noalias() += Lth * theta_.value();
209 xs[i + 1].noalias() += Yth * theta_.value();