From 0706dc0787674ca6bca926e986113aafecfba333 Mon Sep 17 00:00:00 2001 From: Peter Wu Date: Thu, 16 Jan 2014 16:45:49 +0100 Subject: Better document SmarterWalkAnimation, remove DumbWalkAnimation The arm rotation angles are now based on the feet position. Better explain rationale instead of writing the implementation in comments. --- src/DumbWalkAnimation.java | 65 ---------------------------------------- src/SmarterWalkAnimation.java | 69 +++++++++++++++++++++++++++---------------- 2 files changed, 43 insertions(+), 91 deletions(-) delete mode 100644 src/DumbWalkAnimation.java diff --git a/src/DumbWalkAnimation.java b/src/DumbWalkAnimation.java deleted file mode 100644 index 5dc2ac1..0000000 --- a/src/DumbWalkAnimation.java +++ /dev/null @@ -1,65 +0,0 @@ - -/** - * A WalkAnimation that does not result in a smooth transition. The formula and - * constants are chosen at random. - * - * @author Peter Wu - */ -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; - } - - private double getTime() { - return robot_pos_meters / 5; - } - - @Override - public double getLegAngleLeft() { - return Math.sin(-getTime()) * 30.0; - } - - @Override - public double getLegAngleRight() { - return -getLegAngleLeft(); - } - - @Override - public double getKneeAngleLeft() { - return 75.0 + Math.abs(Math.cos(getTime()) * 90.0); - } - - @Override - public double getKneeAngleRight() { - return getKneeAngleLeft(); - } - - @Override - public double getArmAngleLeft() { - // static non-moving arms. - return 0; - } - - @Override - public double getArmAngleRight() { - // static non-moving arms. - return 0; - } - - @Override - public double getBottomOffset() { - return legLength; - } -} diff --git a/src/SmarterWalkAnimation.java b/src/SmarterWalkAnimation.java index bf4e72e..64f3626 100644 --- a/src/SmarterWalkAnimation.java +++ b/src/SmarterWalkAnimation.java @@ -62,47 +62,64 @@ public class SmarterWalkAnimation implements WalkAnimation { 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...). - */ + // 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 = new Point(.25, 0.0); - Point foot_r = new Point(.25, 0.0); - double support_dist; + 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) { - foot_r.x = -.25 + t; - foot_r.y = FOOT_MAX_LIFT * sin(t * PI); - support_dist = foot_r.x - body.x; + // supported by left, so the right foot is moving. + foot_l = supporting_foot; + foot_r = moving_foot; } else { - foot_l.x = -.25 + t; - foot_l.y = FOOT_MAX_LIFT * sin(t * PI); - support_dist = foot_l.x - body.x; + foot_r = supporting_foot; + foot_l = moving_foot; } - // 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); + double foot_angle_right, foot_angle_left; // base rotation for legs - leg_angle_right = calcAngle(body, foot_r); - leg_angle_left = calcAngle(body, foot_l); + 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); - // correct the foot position due to the bent leg + + // 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); - arm_angle_left = MAX_ARM_ANGLE_DEG * cos(pos / 2 * PI); - arm_angle_right = MAX_ARM_ANGLE_DEG * sin(pos / 2 * PI); + // 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; } @@ -129,12 +146,12 @@ public class SmarterWalkAnimation implements WalkAnimation { @Override public double getArmAngleLeft() { - return arm_angle_left; + return arm_angle_left * 180 / PI; } @Override public double getArmAngleRight() { - return arm_angle_right; + return arm_angle_right * 180 / PI; } @Override -- cgit v1.2.1