From 84284be382b0a32dff0294e07054a921a87943ac Mon Sep 17 00:00:00 2001 From: Peter Wu Date: Mon, 13 Jan 2014 23:28:57 +0100 Subject: Initial working implementation of SmarterWalkAnimation --- src/DumbWalkAnimation.java | 12 +++ src/Robot.java | 13 +-- src/SmarterWalkAnimation.java | 185 ++++++++++++++++++++++++++++++++++++++++++ src/WalkAnimation.java | 7 ++ 4 files changed, 212 insertions(+), 5 deletions(-) create mode 100644 src/SmarterWalkAnimation.java diff --git a/src/DumbWalkAnimation.java b/src/DumbWalkAnimation.java index 7ff44d1..5dc2ac1 100644 --- a/src/DumbWalkAnimation.java +++ b/src/DumbWalkAnimation.java @@ -8,10 +8,16 @@ public class DumbWalkAnimation implements WalkAnimation { private double robot_pos_meters; + private final double legLength; + + DumbWalkAnimation(float legTopLength, float legBottomLength) { + this.legLength = legTopLength + legBottomLength; + } /** * Sets the new position for the robot. */ + @Override public void updatePosition(double pos) { this.robot_pos_meters = pos; } @@ -35,6 +41,7 @@ public class DumbWalkAnimation implements WalkAnimation { return 75.0 + Math.abs(Math.cos(getTime()) * 90.0); } + @Override public double getKneeAngleRight() { return getKneeAngleLeft(); } @@ -50,4 +57,9 @@ public class DumbWalkAnimation implements WalkAnimation { // static non-moving arms. return 0; } + + @Override + public double getBottomOffset() { + return legLength; + } } diff --git a/src/Robot.java b/src/Robot.java index 5e24a36..612ecec 100644 --- a/src/Robot.java +++ b/src/Robot.java @@ -87,7 +87,8 @@ class Robot extends BetterBase { this.boneSize = 0.02f; this.depth = 0.24f; this.laneNo = laneNo; - this.walkAnim = new DumbWalkAnimation(); + this.walkAnim = new SmarterWalkAnimation(legLength / 2, + legLength / 2 + footHeight); } /** @@ -102,14 +103,16 @@ class Robot extends BetterBase { gl.glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, material.diffuse, 0); gl.glMaterialfv(GL_FRONT, GL_SPECULAR, material.specular, 0); + // calculate rotation angles and positions for movements. + walkAnim.updatePosition(robot_pos_meters); + // save positions so it can be restored easily later gl.glPushMatrix(); // position the center of the torso above the Z axis such that the foot // stands on the XY plane. - gl.glTranslatef(0, 0, torsoHeight / 2 + legLength + footHeight); - - // calculate rotation angles and positions for movements. - walkAnim.updatePosition(robot_pos_meters); + gl.glTranslatef(0, 0, torsoHeight / 2); + // the length of the legs and foot are included. + gl.glTranslated(0, 0, walkAnim.getBottomOffset()); // Draw the robot, everything is relative to the center of torso. diff --git a/src/SmarterWalkAnimation.java b/src/SmarterWalkAnimation.java new file mode 100644 index 0000000..9b4d7f0 --- /dev/null +++ b/src/SmarterWalkAnimation.java @@ -0,0 +1,185 @@ + +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 final double MAX_LEG_ANGLE_DEG = 40.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; + + 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?!"; + + /** + * At t=0, the body is at x=0; the right foot at x=-1/4 and the left + * foot at x=1/4. During the transition from t=0 to t=1/2, the right + * foot accelerates to x=3/4 while the left foot stays at x=-1/4 + * (something has to support the robot while it is moving...). + */ + Point body = new Point(t / 2.0, 1.0); + Point foot_l = new Point(.25, 0.0); + Point foot_r = new Point(.25, 0.0); + double support_dist; + if (supported_by_left) { + foot_r.x = -.25 + t; + foot_r.y = FOOT_MAX_LIFT * sin(t * PI); + support_dist = foot_r.x - body.x; + } else { + foot_l.x = -.25 + t; + foot_l.y = FOOT_MAX_LIFT * sin(t * PI); + support_dist = foot_l.x - body.x; + } + // base the robot body bottom position on the foot that is supporting + // the robot. + body.y = sqrt(1.0 - support_dist * support_dist); + + // 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); + + // base rotation for legs + leg_angle_right = calcAngle(body, foot_r); + leg_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); + // correct the foot position due to the bent leg + leg_angle_left += cosineRule(legTopLength, dist_leg_l, legBottomLength); + leg_angle_right += cosineRule(legTopLength, dist_leg_r, legBottomLength); + + 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 0; + } + + @Override + public double getArmAngleRight() { + return 0; + } + + @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; + } + } +} diff --git a/src/WalkAnimation.java b/src/WalkAnimation.java index f66e7df..33cbce1 100644 --- a/src/WalkAnimation.java +++ b/src/WalkAnimation.java @@ -62,4 +62,11 @@ interface WalkAnimation { * @return angle in degrees. */ public double getArmAngleRight(); + + /** + * Finds the distance between the floor and the bottom of the body. + * + * @return A distance in meters. + */ + public double getBottomOffset(); } -- cgit v1.2.1