One good way of generating a centre of mass trajectory is the quadratic programming method proposed in [1].

The idea pioneered in [2]  is to generate a centre of mass trajectory based on a zero moment point trajectory, in turn the zero moment point trajectory can be generated by deciding the foot placement positions(as shown in the figure to the right, sometimes theres little choice in feasible footstep positions) and making a ZMP trajectory that is always inside the support region. Centre of the support foot during single support phase and a 5th order polynomial between the two feet during double support phase is one option(see the appendix at the bottom for more on polynomial trajectories).

The idea in [1] was to do the same thing as [2] but via quadratic programming. This has some advantages such as allowing for constraints on the ZMP position and, in my experience, being easier to implement.

First I am going to explain how trajectories can be generate using quadratic programing in general and then I will explain the specifics of how to it can be used to generate a COM trajectory.

Given a linear system
$X_{k+1}=AX_{k}+Bu_{k}$
$z_{k}=CX_{k}$

Here X is a 3×1 control variable as follows
$x=\left[\begin{array}{c} x\\ \dot{x}\\ \ddot{x} \end{array}\right]$
and u is the jerk i.e $u=\dddot{x}$

N values of the systems output can be described by the following linear system
$\bar{Z}=\bar{M}X_{0}+\bar{N}\bar{u}$

where $X_{0}$ is the initial state of the system, $\bar{M}$ and $\bar{N}$ are as follows.
$\bar{M}=\left[\begin{array}{c} CA\\ CA^{2}\\ \vdots\\ CA^{N} \end{array}\right]$
$\bar{N}=\left[\begin{array}{cccc} C & 0 & \ldots & 0\\ CAB & CB & & 0\\ \vdots & & \ddots & \vdots\\ CA^{N-1}B & CA^{N-2}B & \ldots & B \end{array}\right]$

The objective is to make the systems outputs track a certain goal while minimizing actuator effort. This objective can be described mathematically as follows
$J=\frac{1}{2}Q(\bar{Z}-\bar{Z}^{ref})^{2}+\frac{1}{2}R\bar{\dddot{x}}$
$\bar{Z}=\left[\begin{array}{c} z_{0}\\ z_{1}\\ \vdots\\ z_{N} \end{array}\right]\\$$\bar{Z}^{ref}=\left[\begin{array}{c} z_{0}^{ref}\\ z_{1}^{ref}\\ \vdots\\ z_{N}^{ref} \end{array}\right]$$\bar{X}=\left[\begin{array}{c} X_{0}\\ X_{1}\\ \vdots\\ x_{N} \end{array}\right]$

Here Q and R are the performance and cost indexes respectively and $z_{k}^{ref}$ represents the desired output state at time k. The above can be solved as a quadratic program by rearranging it into the form shown below

$\underset{\bar{\dddot{x}}}{min}\quad f(\bar{\dddot{x}})=\frac{1}{2}\bar{\dddot{x}}^{T}w\bar{\dddot{x}}+v\bar{\dddot{x}}$

Here w and v are as shown below
$w=QN^{T}N+RI_{N\times N}$
$v=QN^{T}Mx_{0}-QN^{T}\bar{Z}^{ref}$

The above quadratic program can be solved simply as follows
$\dddot{x}=-w^{-1}v$

Now that the general method of generating trajectories using quadratic programming has been discussed, ZMP preview control can be realized simply by using the following A,B and C matrices
$A=\left[\begin{array}{ccc} 1 & T & \frac{T^{2}}{2}\\ 0 & 1 & T\\ 0 & 0 & 1 \end{array}\right]\label{eq:AMat}$

$B=\left[\begin{array}{c} \frac{T^{3}}{6}\\ \frac{T^{2}}{2}\\ T \end{array}\right]$

$C=\left[\begin{array}{ccc} 1 & 0 & -h_{com}/g\end{array}\right]$
Here $h_{com}$ is the center of mass height, g is the gravity constant, T is the sampling period. The A and B matrices implement a integrator system and the C matrix implements a ZMP output.

A step ZMP input should provide the following COM trajectory response. Note how the COM starts moving before the ZMP reference value changes so as to minimize global ZMP error.

Here is a short Matlab implementation.

Next: ZMP Stabilization

References

[1] “Trajectory Free Linear Model Predictive Control for Stable Walking in the Presence of Strong Perturbations” Pierre-Brice Wieber
[2] “Biped Walking Pattern Generation by using Preview Control of Zero-Moment Point” Shuuji Kajita, Fumio Kanehiro, Kenji Kaneko, Kiyoshi Fujiwara, Kensuke Harada, Kazuhito Yokoi and Hirohisa Hirukawa, Proceedings of the 2003 IEEE International Conference on Robotics & Automation, Taiwan, 2003

Appendix

A  5th order polynomial is a useful tool for defining trajectories as a function of the start and end positions, velocities and accelerations. It is defined as follows.

$x(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$

where the coefficients are calculated as follows

$\begin{array}{c} a_{0}=x_{start}\\ a_{1}=\dot{x}_{start}\\ a_{2}=\frac{\ddot{x}_{start}}{2}\\ a_{3}=\frac{20x_{end}-20x_{start}-(8\dot{x}_{end}+12\dot{x}_{start})t_{end}-(3\ddot{x}_{start}-\ddot{x}_{end})t_{end}^{2}}{2t_{end}^{3}}\\ a_{4}=\frac{30x_{start}-30x_{end}+(14\dot{x}_{end}+16\dot{x}_{start})t_{end}+(3\ddot{x}_{start}-2\ddot{x}_{end})t_{end}^{2}}{2t_{end}^{4}}\\ a_{5}=\frac{12x_{end}-12x_{start}-(6\dot{x}_{end}+6\dot{x}_{start})t_{end}-(\ddot{x}_{start}-\ddot{x}_{end})t_{end}^{2}}{2t_{end}^{5}}\end{array}$

### 37 Responses to “Trajectory Generation”

1. if you remove line 12 in your code won’t the program work?

• Indeed, thank you. It was a remnant of earlier work. I updated the page along with some other corrections.

2. SIr,how you are taking ref zmp?how to take the value of N?

• I just made a square wave with lines 12&13 of the code. In practice you would want to keep the ZMP inside the support area created by the feet then the code will generate an appropriate COM trajectory. The product N*T should be greater than 1.6 seconds, you can try different values and see how it works out but the algorithm may fail to converge for some values. If I remember right [1] was suggesting N=30 with a high T value for the purpose of generating the trajectory in realtime.

• how to generate stable gait and gait trajectory..?

• Thats the whole point of the algorithim described on this page. It will generate a COM trajectory that will minimizes the error between the resultant ZMP and the refferential ZMP. As Long as you selected the refferential ZMP to lie within the support area at all times, the gait should be stable. Please read [1] for more details.

• Also, take note that this algorithm will give you a COM trajectory. You have to convert it to a joint trajectory via COM based inverse kinematics.

• sir how to keep zmp in support area of the feet?

• I cant give you a good answer if you keep making one line replies. If your question is about generating the ZMP time profile then that a easy matter conceptually but a tricky one in terms of implementation. Conceptually you want the ZMP to be at the center of the foot during single support(making it move forward makes a more natural walk but its a bit more difficult to stabilize). Then in double support you can use a 5th order polynomial to shift the ZMP from the centre of one foot to another. Implementation wise, check out my project on sourceforge, http://www.sf.net/p/robotjoint . The Robot.cxx file in the src directory should have a MakeZMPTrajectory() function that implements it. Based on this discussion I think I should expand this page to explain more details( when I have time, which wont be for a long while), I simply explained what I consider to be the hardest part on this page.

• In this paper ‘http://hal.inria.fr/docs/00/39/04/62/PDF/Preview.pdf’ N = 300, T = 5ms.

3. Hello,

First, great explanations. It helps a lot to understand what no one seems to explain on papers. I am trying to study biped robot gait control and planning to use OpenHRP3. I just created an IPC using TCP/IP to control my robot using simulink so i don’t have to worry with C++ math. However, I am looking for a long time now for a good solution on how to generate the IK of my robot so i can calculate joints positions (and velocities) from a foot or COM trajectorie. Could you give me any tip where to find a good IK tool for biped robots? The only tools that i could find that seems to be usable are OpenRave’s IKfast and Orocos numeric IK.
Also do you have any matlab examples with your ZMP implementation?

Best regards,

Luís Filipe Rossi

• Hey Luís

IPC over TCP/IP, thats a great idea! why didn’t I think of doing that … my only concern is the run speed of doing that and doing calculations in Matlab. If you feed your robot a trajectory sequence and it executes it locally then it should be fine though.

Well I haven’t tried Orocos yet, IKFast is awesome but I dont think either Orocos or IKFast can handle COM IK for a humanoid robot.
I wrote a solver that works well enough for humanoids, although my code isnt very mature yet. Im not aware of any other projects that are suitable for humanoid robots but I havent searched much either. Anyway you can find my solver here http://sourceforge.net/p/robotjoint/code/ci/master/tree/src/Robot.cxx#l684 (its not beautiful but it works). With that function you basically supply the desired left foot to COM vector (everything is in the left foots coordinate frame) and the left foot to right foot vector(this approach works with both left and right support phases, just requires a little math manipulation) then it will calculate the joint angles for you.

Sorry I dont have a matlab implementation of the ZMP generation algorithm but you can find the c++ implementation here
http://sourceforge.net/p/robotjoint/code/ci/master/tree/src/Robot.cxx#l369 . I basically start by deciding the foot positions then I construct a sequence of support phases and finally I convert this sequence into a ZMP trajectory.

If you want to give my c++ project a try(maybe you can try that IPC trick again to connect it with your other software, would require some coding though) then I would recommend getting the latest version from the git repository. I think it wont compile because it will complain about a missing file (“urn00b.h” or something like that), just delete the line that refers to that file and it will compile.

Good luck and hope it helps.

• Hello Kirill,

Well it is much slower than the pure C++ implementation and not useful for real world robots. However, for concept testing with simulations is great. As i can make the control sychronous to the penHRP3 controller i dont need to worry about long delays. I was using Gazebo with ROS and that was not possible…

For sure i will take a look on your implementation of both IK and full ZMP. One question, where should i define my kinematic chain?

ZMP is basically a standard in biped robot world and it is very weird that there are no common tools for biped robot IK.

Well thank you for your help!

Best regards,

Luis Filipe Rossi

• Hey Luis

Yup, its great for offline work.
Whats your opinion of ROS? Is it useful for bipeds or is it mostly for wheeled robots and computer vision stuff?
Is penHrp3 a hardware controller? If so, can it be adapted to work with custom robots or does it have to be hrp?

As for defining you kinematics. Have a look in TestSuiteFiles/TestRobot.cxx (I think…).
In the constructor of that file a robot is defined by using addManipulator to make arms/legs and then addJoint is used on the manipulator to define the joints. It should work with standard DH paramaters, im using the transform matrices defined by Sprong. ( Modeling & Control… maybe you know the textbook? ).
Also in that file there is a TestAll method, in it addMovementPhase can be used to define your foot positions and places where single and double support happens. Then MakeZMPTrajectory and MakeCOMTrajectory can be used and finally MakeJointTrajectory should do the inverse kinematics.
One problem though (aside from the code being experimental :] ) is that I didnt implement turning yet.

I think there is still a lack of tools for humanoid robotics, its a bit disappointing but I think this field is still in its infant stage so this is to be expected.

Anyway feel free to let me know if you need any help. Robotics is one of my favorite topics 🙂

• Hello again,

First of all. I tried to compile your code (at ubuntu x64) and my GCC is throwing an error about some casts that are losing precision. Should i add -fpermissive to my Makefile?

Sorry it is not penHRP3, it is OpenHRP3. I was referring to the controller i will be using with the simulator. Well, in general i liked ROS, the topics concept is very useful for portability. Gazebo is not very mature in my opinion. It is getting better but it was a hell on earth during DRC competition because it was changing versions with significant changes all the time. Rosen (from OpenRave) told me that OpenHRP3 is more precise for gait simulation compared to Gazebo (and OpenRave). Still i could not find any direct comparison, after a small research i believe he is correct (even though i am not even close to an expert in numeric simulation.. i am an embedded system engineer).

Do you have any other good reference about ZMP implementation? I believe what would be very helpful is some simple but complete example, showing all math steps.

Thank you for all your help.

Regards,

Luis

• Hi Luis

Sorry to say but I never tested it on a 64bit system. You can try stuffing the permissive option in but im not sure what will happen.

There is a textbook written by Dr. Kajita( the chief humanoid robotics researcher at AIST) but its in Japanese…

Well making a ZMP trajectory isnt too difficult conceptually, but it can be a bit tricky to do in a program. You just gotta decide your support phases(left, right and double support) and the foot positions at those phases. Then during the single support phases you keep the ZMP at the centre of the supporting foot and in double support you use a 5th order polynomial to move the ZMP from one foot to another. Once you done that, then you can use the matlab script I put on this page to get a COM trajectory. Finally you can transform from global coordinates to left foot coordinates and use the COM inverse kinematics solver in RobotJoint. Thats a rough explanation of how I do it. If you want to try implementing it yourself, then please feel free to ask me if you get stuck.

I see, thank you for the info. Gazebo sounds like a interesting simulation environment, my only problem is that I dont think it provides a lot of stuff for bipeds.
Im not a expert on the numerical simulation stuff eiether but when I researched this stuff some years back I got the impression that some software uses a slow but accurate newton-euler method for calculating joint accelerations while other software used some other faster(game software related) but less accurate methods. I think OpenHRP should be quite good because the guys at AIST use it to test their code before trying it on the HRP robots.

Good luck and feel free to ask me for help anytime 🙂

4. Hey Kiriil,

Very nice explanation, Could you please explain more about matrix A and B and how you obtain them? What do you mean exactly by and integrator system?

Thank you 🙂

• Hi Mahyar
Thank you for your question and Im glad you like the explanation although I think my explanations on this site arent very good, they need to be cleaned up. Fortunately I might get the time to do that soon.
The A and B matrices basically integrate the position, velocity and acceleration of the COM based on the jerk of the system. So the structure of A and B follows from the equations of motion. For example, end_position = start_position + dt*velocity + (1/2)*dt*dt*acceleration + 1/6*dt*dt*dt*jerk, thus the first row of A is [1 dt 0.5*dt*dt] and the first row of B is 1/6*dt*dt*dt. Then the second row can be found by integrating the velocity and so on. Here is a link that might be useful.
http://physics.info/kinematics-calculus/practice.shtml
I would also recommend checking the paper “Online walking gait generation with adaptive foot positioning through linear model predictive control” by H. Diedam et al. as it has a more advanced version of the algorithm I presented here.

• Thank you very much:) I’m also wondering what is the intuition behind choosing jerk as the input of the system? Is it related to the equation of motion that you said ? Why we don’t choose for example acceleration as input?

• Hi Mahyar
Well im not so sure right now but I think one advantage of using jerk instead of acceleration is that the acceleration will be continous even though the QP algorithim may produce a discontinuous jerk series. If the QP was set up to output an acceleration instead of a jerk then the acceleration may be discontinuous, in this case the ZMP would also be discontinuous. In turn this may make it hard to stabilize the ZMP to its referential value especially if derivative control is used.

• thanks for answering my questions.So, I came up with another question, there are also analytical methods to solve this zmp equation like the one :” An analytical method on real-time gait planning for a humanoid robot”. i’d like to know what is the advantage of using preview control or quadratic programming over these analytical methods? aren’t analytical methods even better becuase of low computations and online pattern generation without any need to preview signal?

• Thanks for your interesting question. The main advatage of using QP is that constraints such as ZMP, foot collision, foot velocity, etc can be explicitly taken into account(but note that I havent done it in the above example). Meanwhile the main advantage of Preview control was that the COM could start moving before the ZMP reference changed instead of moving reactively. This had an effect of reducing the total ZMP tracking error, but I think the paper you cited achieves a simmilar effect through a least squares approach. I should say that both the paper you cited and what I have shown here are analytic solutions, no iteration is required and convergance is gauranteed. In order to make this method work in realtime you will need to set N to a low number like N=24 while keeping the preview time above 1.6 seconds(N*T>1.6) and repeat the planning process every sampling step in a model predictive control fashion. There are other tricks like pre-factoring the cholesky decomposition but I found it unnecessary. Regardless, if you plan on using this method on a DSP then that might not work. Also, its not difficult to get the preview data, you just need to know the step positions in advance(but foot positions need not be constant in a MPC loop), I think the method you cited also requires knowledge of the foot positions in advance. I recommend reading a PhD thesis by Benjamin Stephens from CMU (I think his title was something like, Humanoid Robot Push Recovery). He demonstrated a QP based MPC framework for simultaneously planning the robots COM and foot positions(reactive stepping) in realtime. He also mixed in some cool things like CMB balancing and capture point tracking. The nice thing about QP is that it is general enough to handle all that.

5. Your discussion on ZMP and bipedal robots in general is very helpful! I have been struggling with getting a firm understanding of the concept and I think you managed to clear it up.

You have great content in this website, and I’m enjoying everything in it so far. Keep it up please!

6. In the http://www.elysium-labs.com/robotics-corner/learn-robotics/biped-basics/zero-moment-point-zmp/ you say ZMP is calculated as xCom – (zh/g) * xCOM” which is the acceleration of xCOM if i am correct.

If so then how come in matlab code you write
zk=C*xk;
Xk=[[Xk];xk’];

C = [1 0 -hcom/g]

If I expand it this would give me xCOM – (zh/g)*xCOM … not xCOM” , so i’m asking if there isn’t a bug in the calculation.
Because I am having troubles calculating the ZMP from COM.

• Xk is a vector not a scalar, it contains the com position velocity and acceleration.

7. Do you need to do this code for y too?

• Yes but I hope you will investigate Model Predictive Control for humanoid robots more deeply. What I wrote here is only meantfor informal education.

8. very good website for starters, thank u very much krill
I am new to biped robots, my question is I want to generate stable trajectories and I want to use LQR as well,
what I have done s far in the forward kinematics and dynamics of the biped model, can u please tell me what I need to do further,

9. actually I am stuck here and things are getting pretty mixed up please if u can tell me what to do next

10. hey,

I have another question as well;
what if I want to generate stable trajectory using the simplified cart table model (calculating the cart com trajectory to realize the given zmp pattern) then use LQR with cart table com to track the ZMP.

let me put my question another way;
I want to use the cart table model (approximation in stead of using full robot model) to generate a COM trajectory and Then find the ZMP and then make the COM of cart table model to track the given ZMP.

please tell me how to start or what step by step procedure to follow

• Hi Rameez

Sorry If I dont reply quickly or if I forget to reply, I just have a lot to do.

I will try to help you with your question but please dont expect a step-by-step explanation, im not going to do your homework for you and I dont have enough time for that anyway.

Ok so you want to make a ZMP stabilizer based on LQR?
I think your main difficulty is in making the LQR controller right? If so then lets assume you have a system with dynamics like so dx＝f(x,u) where x is the state(zmp in your case) and u is the input(like com acceleration or joint acceleration or whatever you choose). One problem is that LQR is only made for tracking a state down to zero, not some reference value but it turns out that if we take a taylor expansion of the function f around a desired point and transform our variables, then we can use LQR to track to a desired state. For example, the taylor expansion of f would be: dx – f(x0,u0) ＝ (df/dx)(x-x0)＋(df/du)(u-u0). So if you take the change of variables y＝x-x0, v＝u-u0, A＝df/dx, B＝df/du
Then we get a linear system dy＝Ay＋Bv from which a LQR controller could easily be made. Oh and dont forget to transform your control variable back once you get a numerical answer from LQR, i.e u＝v＋u0.
Just one thing to be weary of is that solving the Riccati equation in 10 or more dimensions can be difficult so be careful if you want to Consider full body LQR. Ok so in summary, the first thing that you need is the function f(x,u) and its derivatives after that it should be easy.

11. Hi Kirill.

I really appreciate it. you are doing people a great favor

12. for example;
I have a two link robotic leg.
can I generate the trajectory for the end effector (in this case end effector will be the foot) and then track that trajectory using LQR. if yes can you suggest methods for generating foot trajectory for a foot step of certain height and length

please don’t shut me down if I am asking too many questions, obviously I will not ask for solution but I just want to see if certain things are possible or not

• Ok so, in this case you are talking about using LQR for position tracking instead of ZMP tracking, this is easy. If you use inverse kinematics to find a desired joint angle for each link then you can treat this as a very standard type of LQR position control problem at each joint.
Once you know the desired foot positions, you can use two 5th order polynomials to connect them. One polynomial to ascend to the desired rise height and one ploynomial to descend.

13. thank you very much for your time
it helps me a lot.
i will ask again if i have any question

14. hi,

Is the optimal controller proposed in [“Katayama, T., Ohki, T., Inoue, T., Kato, T.: Design of an optimal controller for
a discrete-time system subject to previewable demand. International Journal of Control 41(3), 677{699 (1985)”] is an EXTENSION of LQR,
I read it in a paper (Online Generated Kick Motions for the NAO Balanced Using Inverse Dynamics)
IF YES, please can you briefly explain How

15. another question is can LQR be applied to the cart-table model to show the results as done in the above given code by you