import static java.lang.Math.*; /** * A WalkAnimation that tries to be a bit more natural than the * DumbWalkAnimation. It does so by making the foot follow a sine curve, from * which the knee angles are also derived. * * @author Peter Wu */ public class SmarterWalkAnimation implements WalkAnimation { /** * Length of the upper leg from the groin to the knee. */ private final float legTopLength; /** * Length of the lower leg from knee to the floor. */ private final float legBottomLength; /** * Total length of the leg. */ private final double legLength; private final double cycleLength; private static final double MAX_LEG_ANGLE_DEG = 40.0; private static final double MAX_ARM_ANGLE_DEG = 30.0; /** * Maximum percentage of the legs length to lift the feet. */ private static final double FOOT_MAX_LIFT = .20; /** * Angles in radians. */ private double leg_angle_left; private double leg_angle_right; private double knee_angle_left; private double knee_angle_right; private double bodyOffset; private double arm_angle_left; private double arm_angle_right; SmarterWalkAnimation(float legTopLength, float legBottomLength) { this.legTopLength = legTopLength; this.legBottomLength = legBottomLength; this.legLength = legTopLength + legBottomLength; double maxLegAngle = MAX_LEG_ANGLE_DEG * PI / 180; // single step = sin(maxLegAngle) * legsLength cycleLength = 2 * sin(maxLegAngle) * (legTopLength + legBottomLength); System.err.println("Cycle length (2 steps): " + cycleLength + " meter"); } @Override public void updatePosition(double pos) { // adjust speed pos /= cycleLength; // whether the robot touches the ground with its left foot boolean supported_by_left = pos % 2.0 <= 1.0; double t = pos % 1.0; assert t >= 0.0 : "Time went negative?!"; // The animation is modeled with a transition from t=0 to t=2. // (Actually, the period t=0 to t=1 and t=1 to t=2 have the same // transitions, but with the Right foot and Left foot swapped.) // Picture of half a cycle (t=0.5 to t=1.0 is similar, swapping L and R): // t= 0 0.5 // = = // R L L R // R L L R // = = = // A B C // // In this picture, the left foot stays at position B ("t=0.25") while // the right foot moves from point A ("t=-1/4") to point C ("t=0.75"). // As you may have noticed, the body moves twice as slow as the foot. // (hence body.x=t/2 below). The coordinates of body, foot_l and foot_r // are in unit length (0.0 to 1.0). The body y position has yet to be // determined, but assume that it is located near y=1. Point body = new Point(t / 2.0, 1.0); Point foot_l, foot_r; Point supporting_foot = new Point(.25, 0.0); Point moving_foot = new Point(-.25 + t, FOOT_MAX_LIFT * sin(t * PI)); // Use Pythagoras to calculate the body bottom position from the // leg length (1.0) and body to foot distance body.y = sqrt(1.0 - pow(body.x - supporting_foot.x, 2)); if (supported_by_left) { // supported by left, so the right foot is moving. foot_l = supporting_foot; foot_r = moving_foot; } else { foot_r = supporting_foot; foot_l = moving_foot; } // distance between a bent leg and the body double dist_leg_r, dist_leg_l; dist_leg_l = legLength * distance(body, foot_l); dist_leg_r = legLength * distance(body, foot_r); double foot_angle_right, foot_angle_left; // base rotation for legs foot_angle_right = calcAngle(body, foot_r); foot_angle_left = calcAngle(body, foot_l); // bend the knees knee_angle_right = cosineRule(legTopLength, legBottomLength, dist_leg_r); knee_angle_left = cosineRule(legTopLength, legBottomLength, dist_leg_l); // calculate leg rotations (and consider the bent knee to fit the foot // above the floor) leg_angle_left = foot_angle_left; leg_angle_right = foot_angle_right; leg_angle_left += cosineRule(legTopLength, dist_leg_l, legBottomLength); leg_angle_right += cosineRule(legTopLength, dist_leg_r, legBottomLength); // to balance the robot, rotate the arms in the opposite direction. arm_angle_left = -foot_angle_left; arm_angle_right = -foot_angle_right; bodyOffset = legLength * body.y; } @Override public double getLegAngleLeft() { return leg_angle_left * 180 / PI; } @Override public double getLegAngleRight() { return leg_angle_right * 180 / PI; } @Override public double getKneeAngleLeft() { return knee_angle_left * 180 / PI; } @Override public double getKneeAngleRight() { return knee_angle_right * 180 / PI; } @Override public double getArmAngleLeft() { return arm_angle_left * 180 / PI; } @Override public double getArmAngleRight() { return arm_angle_right * 180 / PI; } @Override public double getBottomOffset() { return bodyOffset; } /*- * Given a trangle: * y| a a * | / | | \ * | /___| |___\ * | b (c) (c) b * -+------------------- x * calculate angle a for length bc and ac. In the left case, the * returned angle (in radians) is negative. */ private static double calcAngle(Point a, Point b) { double ac = a.y - b.y; double cb = b.x - a.x; return atan(cb / ac); } /** * Calculates the distance between two points. */ private static double distance(Point a, Point b) { double side1 = a.x - b.x; double side2 = a.y - b.y; return sqrt(side1 * side1 + side2 * side2); } /** * Given a triangle with sides a, b and c, calculate the angle opposite to * side c. * * @return Angle opposed to side c in radians. */ private static double cosineRule(double a, double b, double c) { // cosine rule: angle c = arccos( (aa + bb - cc) / 2ab ) return acos(((a * a) + (b * b) - (c * c)) / (2 * a * b)); } private class Point { private double x; private double y; Point(double x, double y) { this.x = x; this.y = y; } } }