Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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);

}
Exemplo n.º 3
0
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);
}