// ========== STATIC HELPER METHODS =========== const Pose2D Pose2D::average(const Pose2D& p1, const Pose2D& p2, const double& weight1, const double& weight2) { double divider = weight1 + weight2; double x = (p1.x() * weight1 + p2.x() * weight2) / divider; double y = (p1.y() * weight1 + p2.y() * weight2) / divider; return Pose2D(x, y, Angle::average(p1.getAngle(), p2.getAngle(), weight1, weight2)); }
Vector2d DifferentialDriveKinematics::PartialInverse(const Pose2D<double> &a, const Pose2D<double> &b) { Vector2d diff; diff << b.x() - a.x(), b.y() - a.y(); // This rotation doesn't affect the magnitude of ds, // but it so the sign can work out properly Vector2d dr = a.rotationMatrix().inverse()*diff; double ds = sqrt(dr(0)*dr(0)+dr(1)*dr(1)); if ( dr(0) < 0.0 ) { ds = -ds; } double dtheta = b.heading() - a.heading(); // difference in headings ecl::wrap_angle(dtheta); // diff << ds, dtheta; return (diff << ds, dtheta).finished(); }