search

Digital-Humans-23 / a1

7 stars 0 forks source link

readme

Assignment 1 - Kinematic walking controller

Hand-in: 16 March 2023, 14:00 CET

Leave your name, student ID, ETH email address and URL link to demo video here.

In this assignment, we implement a kinematic walking controller for a legged robot!

Let's see the figure below.

figure: overview

Figure 1: The control pipeline of kinematic walking controller: 1) trajectory planning 2) computing desired joint angles by IK.

We start from five trajectories: one for base, and other four for feet. We plan the trajectories given a target velocity of the robot's base (body) and a timeline of foot contacts (i.e. when and how long does a foot contact with a ground.) The details of how we plan the timeline of foot contacts, and how we generate the target trajectories are out of scope of this assignment. But in Ex.3, we will have a sneak peek of trajectory planning procedure for the robot's base.

Our goal is tracking all the five target trajectories at the same time. We will simplify this problem by assuming the robot's base (somehow...) always perfectly tracks the target base trajectory. Then, we want to find desired joint angles for each leg that allow the foot (i.e. the end effector of individual leg) to reach the corresponding target position. We can effectively formulate this as an IK problem. Since the robot has four legs, by solving four IK problems, we can obtain desired configuration of the robot.

figure: overview2

Figure 2: Don't be frightened! This is a skeletal visualization of our Dogbot. Assuming the base is at the target position, we want to find desired joint angles for each leg that allow the foot (i.e. the end effector of individual leg) to reach its corresponding target position. Note. You can render skeletal view by Main Menu > Draw options > Draw Skeleton.

Hand-in and Evaluation

Once you complete this assignment you should hand in

The grading scheme is as follows

IMPORTANT: If your code is not built successfully, you will get zero point from this assignment. So make sure your code is built without any build/compile error.

IMPORTANT: If the system detect a suspected plagiarism case, you will get zero point from this assignment.

Please leave your questions on GitHub, so your colleagues also can join our discussions.

Exercises

Okay now let's do this step-by-step :)

Ex.1 Forward Kinematics (baseline)

In order to formulate an IK problem, firstly, we have to express the positions of a foot as functions of joint angles ( and the base position). Formally speaking, given a generalized coordinates vector

equation: generalized coordinate

which is a concatenated vector of position of the robot's base, orientation of the robot's base and nj joint angles, we need to find a map between q vector and end effector position pEE expressed in world coordinate frame.

equation: forward kinematics

In the previous lecture, we learned how to find this map by forward kinematics.

Code:

Task:

Details:

Once you implement getWorldCoordinates function correctly, you will see green spheres around feet of the robot.

figure: forward kinematics

Figure 3: Check if your implementation is correct. You should see the green spheres around the robot's feet.

Ex.2-1 Inverse Kinematics - Jacobian by Finite Difference (baseline)

Okay, now we can express the position of the feet as a function of joint angles. It's time to formulate an IK problem: we want to find a generalized coordinate vector qdesired given an end effector target position p EEtarget.

equation: inverse kinematics

In the last assignment, we learn how to formulate the inverse kinematics problem as an (unconstrained) optimization problem.

equation: inverse kinematics as optimization

We can solve this problem by using gradient-descent method , Newton's method, or Gauss-Newton method. Whatever optimization method you choose, we need a Jacobian matrix of the feet point. Remember, Jacobian is a matrix of a vector-valued functions's first-order partial derivatives.

equation: jacobian

For now, we will use finite-difference (FD) for computing Jacobian. The idea of finite difference is simple. You give a small perturbation h around jth component of q, and compute the (i,j) component as follows.

equation: jacobian by finite difference

Code:

Task:

Details:

Ex.2-2 Inverse Kinematics - IK Solver (baseline)

Now, it's time to implement a IK solver. Use Gauss-Newton methods we learned in the previous lecture.

We solve four independent IK problems (one for each leg.) Let's say qdesired,i is a solution for ith feet.

equation: ik for ith foot

We can just solve each IK problem one by one and sum up the solutions to get a full desired generalized coordinates qdesired.

equation: full desired generalized coordinate

Code:

Task:

Details:

// overwrite Matrix J in a single statement
// use eval() function!
J = J.block(0,6,3,q.size() - 6).eval();

Let's see how the robot moves. Run a1App app and press Play button (or just tap SPACE key of your keyboard). Do you see the robot trotting in place? Then you are on the right track!

figure: trotting in place

Ex.3 Trajectory Planning (baseline)

Now, let's give some velocity command. Press ARROW keys of your keyboard. You can increase/decrease target forward speed with up/down key, and increase/decrease target turning speed with left/right key. You can also change the target speed in the main menu.

figure: main menu

Oops! The base of the robot is not moving at all! Well, a robot trotting in place is already adorable enough, but this is not what we really want. We want to make the robot to follow our input command.

Let's see what happens here. Although I'm giving 0.3 m/s forward speed command, the target trajectories (red for base, white for feet) are not updated accordingly. With a correct implementation, the trajectory should look like this:

figure: base trajectory

Code:

Task:

Details:

Once you finish this step, you can now control the robot with your keyboard.

By the way, planning the feet trajectories is a bit more tricky. I already implemented a feet trajectory planning strategy in our codebase so that once you complete generate function, the feet trajectory is also updated by user commands. I will not explain any more details today, but if you are interested, please read the paper, Marc H. Raibert et al., Experiments in Balance with a 3D One-Legged Hopping Machine, 1984. Although this is a very simple and long-standing strategy, almost every state-of-the-art legged robot still uses this simple heuristic. (Sidenote. Marc Raibert, who was the group leader of Leg Laboratory, MIT, later founded Boston Dynamics Company in 1992.)

From now on, we will improve our kinematic walking controller.

Ex.4 Analytic Jacobian (advanced)

Okay, we compute Jacobian matrix with FD. But in general, FD is not only inaccurate but also very slow in terms of computation speed. Can we compute Jacobian matrix analytically? The answer is yes. With a small extra effort, we can derive analytic Jacobian matrix, and implement this into our code.

Code:

Test: Compile and run src/test-a1/test.cpp. Test 4 should pass. Note that passing this test does not necessarily mean you get a full point Your implementation should give correct results for every case. We will auto-grade your implementation with a bunch of test cases after the deadline.

Ex.5 Uneven Terrain (advanced)

Our robot can go anywhere in the flat-earth world. But, you know, our world is not flat at all. Now, we will make our robot walk on an bumpy terrain. Imagine you have a height map which gives you a height of the ground of given (x, z) coordinates (note that we use y-up axis convention i.e. y is a height of the ground.) To make the robot walk on this terrain, the easiest way is adding offset to y coordinates of each target positions.

figure: terrain

Of course, we can simulate rugged terrains but for now, we can just create a bumpy terrain by adding some spheres in the scene as I've done here.

IMPORTANT: For Ex.5, create a new git branch named ex5 and push your code there while your implementation of Ex.1-4 still remains in main branch. If your Ex.5 implementation breaks Ex.1-4, you may not get full points from Ex.1-4.

Task:

Hint:

Bonus

If you are already done with all exercises (and also bored), please try to implement a more complex terrain loaded from a mesh file. If you manage to successfully implement this, you will get extra 5% point (but your final point won't exceed 100%)

IMPORTANT: This may require a lot of code modifications. So please create a new branch called bonus and work there while keep your successful implementation of Ex.1~Ex.4 on the master (or main) branch, and implementation of Ex.5 on the ex5 branch. Again, please make sure your implementation of Ex.1 ~ Ex.5 stays safe in master (or main) and ex5 branch. It's your responsibility to keep the main branch clean and working.

figure: mesh terrain

Task:

Hint:

You can download a terrain file you want to use from any internet source, but if it's hard to find one, you can use one in data directory data/terrain/terrain.obj

Final Note

Congratulations! You can now control the legged robot! Hooray!

Can we use our kinematic walking controller to a real robot? Well... unfortunately it's not very easy. In fact, working with a real robot is a completely different story because we need to take into account Dynamics of the robot. But, you know what? We have implemented fundamental building blocks of legged locomotion control. We can extend this idea to control a real robot someday!

By the way, can you make a guess why using kinematic controller for a real legged robot doesn't really work well in practice? If you are interested in, please leave your ideas on the GitHub issue.