VectorXd LinearizedControlError::operator()(const VectorXd& a) const {
Vector6d x1 = a.topRows(6);
Vector6d x2 = a.middleRows(6,6);
double phi = a(12), Delta = a(13), curvature = a(14);
double phi_ref = cfg0->phi, Delta_ref = cfg0->Delta, curvature_ref = cfg0->curvature;
MatrixXd F = MatrixXd::Zero(6,6);
F.topLeftCorner(3, 3) = -rotMat(Vector3d(Delta_ref*curvature_ref, 0, phi_ref));
F.topRightCorner(3, 3) = -rotMat(Vector3d(0, 0, Delta_ref));
F.bottomRightCorner(3, 3) = -rotMat(Vector3d(Delta_ref*curvature_ref, 0, phi_ref));
MatrixXd G = MatrixXd::Zero(6,6);
G(2, 0) = 1;
G(3, 2) = 1;
G(5, 1) = 1;
MatrixXd A = F.exp();
MatrixXd halfF = 0.5*F;
MatrixXd B = (1/6.0) * (G + 4.0*(halfF.exp()*G) + A*G);
Vector3d u;
u << Delta - Delta_ref, phi - phi_ref, Delta*curvature - Delta_ref*curvature_ref;
return x2 - A * x1 - B * u;
}
VectorXd ControlError::operator()(const VectorXd& a) const {
Matrix4d pose1 = cfg0->pose * expUp(a.topRows(6));
Matrix4d pose2 = cfg1->pose * expUp(a.middleRows(6,6));
double phi = a(12), Delta = a(13), curvature = a(14);
return logDown(helper->TransformPose(pose1, phi, Delta, curvature).inverse() * pose2);
}
VectorXd PoseError::operator()(const VectorXd& a) const {
Matrix4d pose1 = cfg0->pose * expUp(a.topRows(6));
Matrix4d pose2 = cfg1->pose * expUp(a.middleRows(6,6));
double Delta = a(12);
double curvature = a(13);
double theta = Delta * curvature;
Vector6d v;
v << 0, 0, Delta, theta, 0, 0;
return logDown((pose1 * expUp(v)).inverse() * pose2);
}