diff options
Diffstat (limited to 'src/SmarterWalkAnimation.java')
-rw-r--r-- | src/SmarterWalkAnimation.java | 69 |
1 files changed, 43 insertions, 26 deletions
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 |