Klamp't Tutorial: Creating a Trajectory from Keyframes

In this tutorial we learn how to generate a time-parameterized trajectory from keyframes. The app and C++ versions will describe how to generate time-optimal trajectories, while the Python version will only describe how to generate fixed-duration trajectories.

Difficulty: easy

Time: 5 minutes

Note for users that compiled from source: These instructions assume that RobotPose is built with Qt support. For instructions using the GLUI version, please refer to the manual.

First, launch the RobotPose program with your desired robot or world file:

./RobotPose data/hubo_plane.xml

We will begin by saving the start configuration to a resource. Select Add Resource... -> Config, and select it. Let's change its name to something more sensible by double clicking its name, entering "start", and pressing enter. Now, click the "From Poser" button, which stores the current configuration into the "start" resource.

RobotPose screen 1

Now let's pose an end configuration. Use the poser (right-drag) to move the robot's arms around to some pose of your choosing.

RobotPose screen 2

Repeating the steps above, save the configuration to another Config resource, and name it "end".

The next step combines the resources as milestones in the Configs compound type. To do so, select Add Resource... -> Configs, and drag the "start" and "end" resources under it. Click the "+" button to expand the resource tree to make sure they are in the right order, as follows.

RobotPose screen 3

Next, select the Configs[2] resource and click the Optimize button in the bottom right. (You may also play around with the "Num divs" parameter, which will control the number of divisions the optimization routine will use when discretizing the path. A finer discretization results in more optimal paths but takes longer to optimize.)

Voila! you will see a new resource, named MultiPath[1]. Select this resource and press play to watch the trajectory animated.

RobotPose screen 4

At this point, you can go ahead and save the resource to disk. The saved MultiPath file can be read into the SimTest program. (You may also choose Convert to... -> Linear Path to convert it into a linear path type before saving. This is useful to use the Python Klamp't trajectory controllers, which read linear path files.)

Let's look at a typical usage case: you've generated a motion, tested it in simulation or on a real robot, and found a problem part-way through the motion. How will you edit the motion?

We'll start by creating a new intermediate keyframe and then editing it to a new desired configuration. Select the MultiPath resource, and move the playback slider to some intermediate configuration.

RobotPose screen 5

Next, click the "To poser" button, which will update the poser to the currently interpolated robot configuration. Pose this robot as desired, and save the posed robot to a new Config resource, called "middle". Drag the resource into the Configs[2] resource and place it between "start" and "end", as shown here.

RobotPose screen 6

Now, select the Configs[2] resource and click Optimize again. A new MultiPath pops up. Pressing play will show that the new path passes through the "middle" milestone before continuing on to "end".

RobotPose screen 7

That's all there is to it! If you'd like to uniformly speed up or slow down the motions you may edit the Duration parameter, or for more detailed control you can edit the timing of the MultiPath by expanding it into its component resources and editing the "times" sub-resource.

Klamp't provides two methods for generating trajectories from keyframes: piecewise-linear motions (stopping and starting at each keyframe), or spline-interpolating motions. In both cases, it provides functionality for optimizing the duration of the path. Each method is implemented via two different mechanisms.

Piecewise linear trajectories

The benefit of piecewise linear motions is that the output of kinematic motion planners (a sequence of configurations whose intermediate segments are collision-free) can be directly converted to a trajectory and sent to the robot, without worrying that it is collision free. They are also blazingly fast to compute (microseconds). The drawback is that these motions need to start and stop at each keyframe, making them rather slow to execute. Moreover, the solver cannot handle contact constraints.

The DynamicPath class in Klampt/Modeling/DynamicPath.h performs all the functionality that we need in just a few lines of code: (The following code assumes the Klampt directory is in your include path)

#include "Modeling/DynamicPath.h"
#include <vector>
using namespace std;

//Returns true if a solution is found successfully.
//May return false if any of the input keyframes are
//out of the robot's joint limits, or the solver encounters
//some numerical issue (quite rare).
//If you wish to disregard joint limits, simply comment out the
//second line.
bool PiecewiseLinearInterpolate(const Robot& robot,
                                const vector<Config>& keyframes,
                                DynamicPath& trajectory)
    return trajectory.SetMilestones(keyframes);

If you wish to convert this path to a MultiPath or Linear Path (e.g., to simulate in SimTest), you may do so using the functions in Klampt/Modeling/Paths.h. An example is as follows:

#include "Modeling/Paths.h"
#include <fstream>

... //include above code here

int main(int argc,const char** argv)
    Robot robot;
    ... //load robot here or at least set up its velocity
        //acceleration, and joint limits
    DynamicPath dpath;
    vector<Config>& keyframes;
    ... //set up keyframes here

    if(!PiecewiseLinearInterpolate(robot,keyframes,dpath)) {
        cerr<<"Error performing interpolation, quitting..."<<endl;
        return 1;

    //create a multipath (exact conversion)
    MultiPath mpath;

    //create a linear path with timing resolution 0.01
    LinearPath lpath;
    double res = 0.01;
    ofstream out("my_keyframe_trajectory.path");

    return 0;

(Note that the conversion to a linear path is not a one-to-one conversion and instead requires a discretization parameter.)

That's all there is to it!

Advanced topic: try to solve the problem that the path starts and stops at each milestone. Note that you can't just smooth the path willy-nilly (e.g., by cutting corners), because this would potentially cause collisions to occur. To see how to smooth the path while still avoiding obstacles, please steps 2 and 3 in the DynamicShortcut function in the file Klampt/Examples/dynamicplandemo.cpp.

Smooth spline trajectories

Spline trajectories solve some of the problems of piecewise-linear trajectories, but optimizing the trajectory timing is more time-consuming. Nevertheless, Klamp't has some of the fastest algorithms currently available for doing so, running in seconds for most typical cases.

The key file to import is Klampt/Planning/RobotTimeScaling.h, which contains routines to optimize trajectories, with or without contact constraints.

The preferred method is as follows. First construct a MultiPath with your keyframes:

#include "Modeling/MultiPath.h"
#include "Planning/RobotTimeScaling.h"

Robot robot;
vector<Config>& keyframes;
... //set up your robot and keyframes here
MultiPath path;
Then, call GenerateAndTimeOptimizeMultiPath as follows:
//Path discretization resolution -- it controls
//how many points N are used in the optimization.
//Running time is empirically quadratic in N
double xtol=0.01;
//Output path discretization resolution
double ttol=0.01;
bool res=GenerateAndTimeOptimizeMultiPath(robot,path,xtol,ttol);
//Path is both input and output, its contents will be
//overwritten with the optimized path.
if(res) {
    cout<<"Optimization successful"<<endl;
else {
    cout<<"Optimization failed"<<endl;

It's as simple as that. You may try playing around with the xtol parameter and observing how the runtime changes, and solution duration (call path.Duration()) changes.

GenerateAndTimeOptimizeMultiPath will also handle contact constraints contained in the path when generating the interpolating curves. Here, you will need to be more careful about the xtol parameter, since it will control how closely the path meets the specified constraints. This will implicitly control the number of optimization points N. [Support for decoupling xtol and N is planned for future implementations.]

Advanced topic: GenerateAndTimeOptimizeMultiPath will not verify that a path is dynamically feasible with respect to torque and contact force constraints. To optimize paths with such constraints, you may use the TrajOpt program (see trajopt.cpp).

The Python API does not currently support trajectory optimization. However, it does allow trajectories to be defined as piecewise-linear, linearly interpolated, or smooth spline functions. For more information, see the documentation on the Trajectory superclass. For now, the following code shows an example of how to create and execute a smooth spline trajectory:

import sys
from klampt import*
from klampt import vis
from klampt.io import resource
from klampt.model import coordinates
from klampt.model import trajectory
import random, time, math

if __name__=="__main__":
    point = coordinates.addPoint("point")
    vis.add("point", point)
    traj = trajectory.Trajectory()
    minDis = -1
    maxDis = 1
    for i in range(10):
        minDis =-1
        maxDis = 1
        val = 1
        if i%2==1:
            val = -1
        traj.milestones.append([(maxDis-minDis)*i/10.0+minDis, 0, val*0.2])
    traj2 = trajectory.HermiteTrajectory()
    vis.animate("point", traj2)

    iteration = 0
    while vis.shown():

Notice that every trajectory has a times and a milestones parameter. In order to create a spline trajectory, you must first create a HermiteTrajectory object and then call makeSpline on a previously created trajectory. This will allow HermiteTrajectory to do the requisite math to convert a series of waypoints into a smooth motion when animated. In this instance, you should see a point moving left to right in a vaguely sinusoidal motion.

Extension: What happen if instead of animating traj2, you animate traj? If you would like to explore this tutorial further please consult the trajectorytest.py file in Klampt/Python/demos

Tutorial 2: Simulation also provides a good tutorial on how to simulate a robot by playing back a saved trajectory. Alternatively, you can send a controller a trajectory that is a collection of either piecewise-linear milestones or just linear milestones. Linear milestones are typically sent time-independent. A simulation just attempts to traverse the trajectory while satisfying its constraints. Piecewise-linear milestone paths, on the other hand, tend to enforce some sort of time constraints for getting from one configuration to another.

Additionally, the script Klampt/Python/utils/multipath_to_timed_multipath.py will convert an untimed MultiPath into a timed one, given a certain number of milestones per second. The speed variable at the top of the script will control the speed of the path.