Klamp't Tutorial: Simulation

In this tutorial we learn how to run a simulation, interact with the simulated robot via the poser, and to simulate paths and trajectories. It is assumed that you have already successfully installed Klamp't.

Difficulty: easy

Time: 5 minutes

Starting and navigating the simulation

First, let's start up a simulation with the NASA ATHLETE robot. From a command line, change directories into the Klampt folder, and run the SimTest program:

./SimTest data/athlete_plane.xml

(If you are running Windows, you will need to add an .exe extension onto the SimTest program name in the command line.)

Here data/athlete_plane.xml is an example of a world file. World files are simple XML files that specify the starting positions of objects, robots, and fixed terrain elements in the world, configure robot controllers, and specify miscellaneous simulation parameters. This is a particularly simple world file that just has the ATHLETE robot standing above a flat ground.

You can also just run SimTest and it will prompt you to choose a world file (unless you compiled without Qt support).

If all goes well you should see a screen like this:

SimTest screen 1

Click and drag on the world with the left mouse button, and observe the view rotating:

SimTest screen 2

Now hold the Shift key and drag up and down with the left mouse button. The view zooms:

SimTest screen 3

Now click on the Play button (green arrow). As the robot hits the ground you should see the orange arrows at the feet briefly extend sharply upward, and then quickly settle into a stable distribution:

SimTest screen 4

Using the poser

Go ahead and navigate around the simulation, holding Shift to zoom and Control to pan. Zoom in on one of the legs of the robot, and click and drag it up and down with the right mouse button. You will see a ghosted yellow robot move its leg to another pose. This is the poser. The grey robot doesn't move... yet.

SimTest screen 5

Next, click on the "Goto" button in the lower left. The robot's leg now moves gradually to the posed configuration.

SimTest screen 6

Now click on the red spring icon to turn on force application mode. Right click and drag on the free limb, and watch the robot get pulled around.

SimTest screen 7

Let the robot settle down, and then pause the simulation with the Pause button (the two grey vertical bars). Switch back to the hand icon, and try right-clicking the center of the 3D posing widget and dragging it around. Note that the poser configuration moves around freely in space.

SimTest screen 8

What happens when you start simulating again?

Nothing! This is because a legged robot is unable to freely move its base without properly applying forces to the environment. This needs to be done via a coordinated motion of all the legs. Let's try this now.

Navigate so that all five legs in contact are in view. Then, click the IK button to bring up Inverse Kinematics Posing Mode. Click on the circle with the cross in the middle, and then right click on all of the legs on the ground. This will freeze the poser's legs in place as the rest of the body is moved.

SimTest screen 9

Now switch to the plain cross, and right-drag on the free limb. This adds a point constraint to the free limb and you can see the body of the robot move to try and maintain the fixed feet constraints.

SimTest screen 10

Press "Goto" and watch the robot move to the desired configuration.

SimTest screen 11

[Note: you may delete previously added IK constraints by choosing the red X and right-clicking on the constraints you wish to discard.]

You're all done! You may continue to play with the poser or continue to the tutorial below on simulating paths.

Simulating paths and trajectories

Motions in Klamp't are specified in configuration space. First, let's take a little detour to talk about configurations...

A configuration of a robot is a vector-like quantity that specifies all joint angles and transformations of free-floating bases of the robot. A configuration is represented as a tuple of D numbers, where D is the dimensionality of the robot. Each number is a degree of freedom (DOF) that maps to a translation or rotation of a link as specified by the Klamp't robot file (.rob or .urdf file).

For example, ATHLETE has 42 DOF, as follows:

hex x, hex y, hex z, hex yaw, hex pitch, hex roll,
hip yaw 1, hip pitch 1, knee pitch 1, knee roll 1, ankle pitch 1, ankle roll 1
...
hip yaw 6, hip pitch 6, knee pitch 6, knee roll 6, ankle pitch 6, ankle roll 6

The first 6 DOF correspond to the transformation of the body of the robot, while the remaining 6 groups of 6 DOF correspond to the joint angles of each leg. It is important to note that order matters! When translating between Klamp't and other modules (e.g., your own robot or ROS) you must take care to map values properly into the convention specified in your Klamp't robot file.

Note that DOF can mostly be treated as independent elements in a Cartesian space, except for the rotation DOF, which must be interpolated in a different way to account for the non-Cartesian nature of the space of rotations. In most typical use cases, Klamp't will take care of these DOF automatically.

...Back to our regularly scheduled tutorial...

Klamp't distinguishes between two types of motions.

  1. The first type, known as a path, is a geometric curve that passes along a list of configurations, but without timing specified.
  2. The second type, known as a trajectory, stores both the path and its time-parameterization.

Depending on your needs, you may want to work with either paths or trajectories. Paths are often simpler to work with because timing is deferred to the robot. On the other hand, trajectories provide the greatest amount of control over the speed of motion, and are preferrable for dynamic tasks.

First, let's look at a simple path, data/motions/athlete_flex.milestones. Open it up in a text editor. You will see it has three lines, and on each line there are two tuples. These are configuration and velocity milestones that the robot should pass through.

(Side note: When stored to a text string, a tuple (x1,...,xD) is a whitespace-separated list of D+1 values:)

D x1 ... xD

Now run the following command:

./SimTest data/athlete_plane.xml -milestones data/motions/athlete_flex.milestones

Using the Display checkboxes, uncheck Poser and check Desired. Now press play. You will see a transparent green robot, which shows the commanded configuration. It almost entirely overlaps the grey robot, but not entirely. Watch the motion play with a smooth interpolation between the milestones.

SimTest screen 12

Now we'll run a time-parameterized trajectory. Run the following command

./SimTest data/athlete_plane.xml -path data/motions/athlete_flex.path

(You may also choose Load Path... from the File menu once the previous path completes)

This file is a Linear Path which is the simplest possible trajectory: a list of milestones that must be met at specified points in time. It is named this way because its timing and movement is given by a piecewise linear interpolation in time and configuration space. Opening it up in a text editor, you will see many lines, each of which contains a time and a configuration.

This particular motion starts at 0s, meets the middle configuration at 1s, and ends at 2s. Let's try playing around with the timing.

First, let's make it slower so that it completes in 10 seconds. Edit the first entry on Line 2 to 5, and edit the first entry on Line 3 to 10. Re-run SimTest or reload the path. Observe how the path is executed slower before.

Now, let's speed up the trajectory so that it completes in 1 second. Edit the first entry on Line 2 to 0.5, and edit the first entry on Line 3 to 1.0. Re-run SimTest or reload the path. Look at it go!

Note that although we made the path faster, it's certainly not as smooth as the milestone path execution. This is because the milestone path smoothly blends between milestones while satisfying the robot's dynamic limits, while a linear path blindly executes the timing that you provide. Thus, a trajectory provides a finer granularity of control, but if you are not careful, it comes at a higher risk of violating constraints and damaging the robot.

Important note: no blending is performed between the robot's start configuration and the start of a linear path.So if the path does not start at the robot's start configuration, then the commanded configuration will jump at the beginning of execution. In summary, linear paths assume that you know what you're doing.

If you are only interested in fixed-base robots or working with robots' arms, feel free to stop here or move on to the Trajectory Tutorial to learn about how you can generate and optimize trajectories. However, if you are interested in legged robots or dexterous manipulation, be sure to read on...

Paths and trajectories with contact

In both of the prior examples, observe that although the start, middle, and end milestones have the same feet positions, the feet move in and out while the robot interpolates between those configurations. This is because the milestones are interpolated in configuration space, without knowing that the feet are supposed stay still. This is quite bad! Not only is the contact being broken, but the robot needs to exert strong forces to maintain the desired configurations (Observe that during simulation, the friction forces point inward and outward to resist the sliding.)

We have provided an example joint space trajectory that has been generated at a finer resolution. Load the simulation with the file data/motions/athlete_flex_opt.path. Although the motion is still interpolated in configuration space, the finer resolution keeps the feet more stationary, which causes less strain on the robot, as seen here.

SimTest screen 13

SimTest can also generate such trajectories automatically with a third motion type: a MultiPath. A MultiPath specifies the constraints on a robot, and can even specify motions that make and break contact. Open up data/motions/athlete_flex.xml in a text editor. You will see an XML file that contains a set of <ikgoal> elements. These are the foot positioning constraints. (Disregard exactly what the text means for now; it's easier to edit these in a GUI.) You will also see a list of milestones, which have a time and a configuration. So essentially, this encodes a linear path plus foot constraints.

If you now load data/motions/athlete_flex.xml, you will now see a bunch of lines written to your console while SimTest does some processing for a few seconds. It's solving for a finely discretized path whose configurations meet the foot constraints. (In particular, the configurations are no more than 0.01 units apart in configuration space, and the foot constraints have no more than 0.001 unit error. Consult the Klamp't manual if you'd like to generate such paths to your own custom tolerances.)

Simulate the path and observe that the foot constraints are indeed kept fixed.

That's it! If you would like to know more about paths, trajectories, and trajectory optimization, you may want to complete the Trajectory Tutorial. You can also play around with the SimTest logging facilities to analyze the results of your motion. Or, proceed to the following sections to learn how to execute simulations programmatically using the Klamp't API.

In this tutorial we'll make a C++ command line program that performs simulation without a GUI.

To make your first app that compiles with Klamp't, first create a folder with two files, main.cpp and CMakeLists.txt.

This very basic main.cpp runs a simulation for 5 seconds (simulation time) from the command line:

#include <Interface/SimulationGUI.h>

int main(int argc,const char** argv) {
  //create a world
  RobotWorld world;

  //The SimGUIBackend class offers many helpers for setting
  //up default simulations, doing logging, sending paths, etc.
  //Advanced users may want to just create a WorldSimulation
  //class for more fine-grained control.
  SimGUIBackend backend(&world);
  WorldSimulation& sim=backend.sim;

  //If you have a fixed world that you want to use, you can
  //load any files like this
  //world.LoadElement("Klampt/data/robots/athlete.rob");
  //world.LoadElement("Klampt/data/terrains/plane.env");

  //Alternatively, this is helpful for loading command
  //line arguments like SimTest
  if(!backend.LoadAndInitSim(argc,argv)) {
    cerr<<"Error loading simulation from command line"<<endl;
    return 1;
  }

  //Uncomment+edit the following line to change the controller
  //time step for robot 0 (100Hz is the default)
  //sim.controlSimulators[0].controlTimeStep = 0.01;

  //Uncomment+edit the following line to change the underlying
  //simulation time step (1kHz is the default)
  //sim.simStep = 0.001;

  //pick some duration between printouts in main loop
  double dt = 0.1;
  //run the simulation
  while(sim.time < 5) {
    //** add control code here **

    //move the sim forward by the given time
    sim.Advance(dt);
    //update the world
    sim.UpdateModel();
    //print time, robot 0's configuration
    cout<<sim.time<<'\t'<<world.robots[0]->q<<endl;

    //Uncomment the following line to log the true state of
    //the robot to disk.
    //backend.DoStateLogging_LinearPath(0,"test_state.path");
  }
  return 0;
}
The CMakeLists.txt file for this example would be the following:
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
SET(KLAMPT_ROOT [Put the path to the Klampt folder here])
SET (CMAKE_MODULE_PATH "${KLAMPT_ROOT}/CMakeModules")
FIND_PACKAGE(Klampt REQUIRED) 
ADD_DEFINITIONS(${KLAMPT_DEFINITIONS})
INCLUDE_DIRECTORIES(${KLAMPT_INCLUDE_DIRS})
ADD_EXECUTABLE(MyApp main.cpp)
TARGET_LINK_LIBRARIES(MyApp ${KLAMPT_LIBRARIES})

Where you specify the Klamp't path in the first line.

Build your file using

cmake .
make

And test it using

./MyApp [Path to Klampt]/data/athlete_fractal_1.xml

If you would like to visualize the path that the robot took, you may save it to a LinearPath by uncommenting the line beginning in backend.DoStateLogging... Recompile, run the simulation, and then run it through RobotPose.

[Path to Klampt]/RobotPose [Path to Klampt]/data/athlete_fractal_1.xml test_state.path

Clicking on the test_state resource and pressing play, you will see the simulation state of the robot sliding partway down the hill.

Note: an alternate way of visualizing the simulation would be to use a GUI system. The simplest way of doing this would be to use GLUI and duplicate the structure of Klampt/Main/simtest.cpp. To implement user-chosen forces or a custom control loop, you should create a subclass of SimTestGUI and overload the virtual function SimTestGUI::OnCommand(const string& cmd,const string& args). When you get a cmd called "advance", implement your control loop and then call return SimTestGUI::OnCommand("advance",args). You can also see the custom controller tutorial for more details.

Sending milestones to the controller

By default, a robot is set up with a polynomial trajectory controller, which stores and executes a smooth interpolating path between keyframes. (See the documentation of PolynomialPathController in Klampt/Control/PathController.h for details.) The preferred, most extensible way of communicating with a robot's controller is with the SendCommand() function, which defines a string-based interface for accepting commands. For example, we can make the robot move its leg pitch (link 7) up after 2 seconds by adding the following lines at the location marked by ** add control code here **:

if(sim.time >= 2.0 && sim.time-dt < 2.0) {
  //move link 7 (hip pitch) 1 radian up
  Config q;
  sim.robotControllers[0]->GetCommandedConfig(q);
  q[7] -= 1.0;
  //LexicalCast is needed to convert config to string
  sim.robotControllers[0]->SendCommand("set_q",LexicalCast(q));
  //then move link 7 (hip pitch) 1.5 radians down
  q[7] += 1.5;
  sim.robotControllers[0]->SendCommand("append_q",LexicalCast(q));
}

Make the program again and run it:

make
rm test_state.path
./MyApp [Path to Klampt]/data/athlete_fractal_1.xml

Note: the rm line is needed because the logging function just appends to the file. To get a clean path from another simulation run, you must delete the existing path file.

Observe the path again using RobotPose. Notice that the path produces a smooth interpolation.

Sending milestones to the controller

Now we will send a trajectory. Replace the previous added lines with the following, which sends a Linear Path piecewise linear trajectory:

if(sim.time >= 2.0 && sim.time-dt < 2.0) {
  //move link 6 (hip pitch) 1 radian up
  Config q;
  sim.robotControllers[0]->GetCommandedConfig(q);
  vector<Real> times(3);
  vector<Config> milestones(3);
  times[0] = 0;
  times[1] = 1;
  times[2] = 2;
  milestones[0] = q;
  q[7] -= 1.0;
  milestones[1] = q;
  q[7] += 1.5;
  milestones[2] = q;
  backend.SendLinearPath(times,milestones);
}

Here, the commanded configuration will meet the milestones at precisely the desired points in time, but with sharp velocity discontinuities. To see this, re-compile, re-run, and observe the path.

Another convenient way to send a path from disk is by calling backend.LoadLinearPath(fn). As a final exercise, try running the athlete_flex.path file starting at t=2s.

Playing God: applying forces and constraining velocities

The robot controller is not able to apply arbitrary forces to its body or the world. This encapsulation is deliberate, because a robot cannot "play God" -- it can only affect its body or the world via its actuators. But it is often useful to generate simulation scenarios by "playing God," and to do so, you must access the underlying rigid bodies in the Open Dynamics Engine (ODE) simulator.

The first step in doing so is to access the ODERigidObject or ODERobot out of the WorldSimulator. To do so, you would call something like this:

ODERobot* simrobot = sim.odesim.robot(my_robot_index);
//or...
ODERigidObject* simobject = sim.odesim.object(my_object_index);

To apply forces, you may use ODE’s API. The way to do this is as follows.

dBodyAddForceAtPos(simrobot->body(my_link_index),fx,fy,fz,px,py,pz);
//or...
dBodyAddForceAtPos(simobject->body(),fx,fy,fz,px,py,pz);

Where the force (fx,fy,fz) and point (px,py,pz) are in world coordinates. For more information, consult the ODE documentation.

Directly controlling the movement of a body (e.g., to move along a predetermined path, or according to a joystick) is possible but takes a few extra steps, because Klamp't by default gives control of the body to the simulator. First, you will need to know the translational and angular velocity along which the body should be moving at each time step. Let us assume you have determined these quantities as (vx,vy,vz) and (wx,wy,wz); both are in world coordinates. Then, you will need to tell ODE to disable dynamic simulation, and during your time step you will need to set the velocities directly as follows:

dBodyID bodyID = simrobot->body(my_link);
//or
dBodyID bodyID = simobject->body();
dBodySetKinematic(bodyID);  // this only needs to be set once at the beginning of simulation
dBodySetLinearVel(bodyID,vx,vy,vz);
dBodySetAngularVel(bodyID,wx,wy,wz);

Extracting contact forces

For research and evaluation purposes it is often useful to record the contact forces generated by the simulation, and Klamp't provides several functions for doing so. This tutorial also illuminates some of the differences between the indexing and body ID schemes.

The first step in extracting contact feedback is to enable it. Contact feedback is enabled on a per body pair basis; this is primarily done to save a little overhead in computation and memory. Each rigid body in the world, including environment objects and robot links, is given a unique ID, and this ID is used to identify the corresponding body in the simulator. To get the ID of an object in the world, you would call:

int terrainid = world.TerrainID(terrain_index);
int objectid = world.RigidObjectID(object_index);
int linkid = world.RobotLinkID(robot_index,int link_index);

IDs are constant throughout the life of the simulation. (NOTE: IDs will change if you add or remove elements from the world -- this is not yet supported in simulation.) IDs are assigned contiguously, and hence it is possible to just loop through integers ranging from 0 to world.NumIDs()-1 to enable all contact pairs. So we will do this before our simulation loop to allow us to observe all contact pairs:

for (int i=0;i<world.NumIDs();i++)
  for (int j=i+1;j<world.NumIDs();j++)
    sim.EnableContactFeedback(i,j);

Don't worry, the computational overhead is not that high. If we really wanted to be selective, we could just do something like this to enable only collision feedback between the terrain and all links on the robot:

for (size_t i=0;i<world.robots[0].robot->links.size();i++)
  sim.EnableContactFeedback(terrainid,world.RobotLinkID(robot_index,i));

But let's assume we just enabled all of them. Then, during the simulation loop, we can use the following code to see what objects are in contact. We can also print out the average contact force/torque:

bool contacted=false;
for (int i=0;i<world.NumIDs();i++)
  for (int j=i+1;j<world.NumIDs();j++) {
    //you could loop over a selective set of id pairs rather than i and j, if you wanted...
    if(sim.HadContact(i,j)) {
      if(!contacted) { printf("Touching bodies:\n"); contacted=true; }
      Vector3 f = sim.MeanContactForce(i,j);
      Vector3 t = sim.MeanContactTorque(i,j);
      printf("  %s - %s: force %g %g %g, torque %g %g %g\n",world.GetName(i).c_str(),world.GetName(j)).c_str(),f.x,f.y,f.z,t.x,t.y,t.z);
    }
  }

If you don't want to get mean contact force/torque, there are other methods of WorldSimulator you can use to retrieve other types of feedback. For example, you can get detailed contact point information using the ContactFeedbackInfo structure:

ContactFeedbackInfo* info = sim.GetContactFeedback(i,j); 

Please consult the detailed API documentation for more information

In this tutorial we'll make a Python simulation program with a GUI. We'll work from the template in Klampt/Python/demos/gltemplate.py. Open up the Python API documentation for reference while you are playing around.

First, copy Klampt/Python/demos/gltemplate.py to myapp.py. If you run

python myapp.py Klampt/data/athlete_fractal_1.xml

and press 's', the simulation will proceed with the robot not doing anything. If you want to simulate while moving the robot around using the keyboard, you can work from kbdrive.py.

Sending milestones to the controller

Now, we will send the same leg lift and lower motion to the controller as we did in the C++ tutorial. Open myapp.py in a text editor. We'll be putting our code in the control_loop method under the comment "#Put your control handler here":

        sim = self.sim
        if sim.getTime() >= 2.0 and sim.getTime()-self.dt < 2.0:
            q=sim.controller(0).getCommandedConfig()
            q[7]-=1.0
            sim.controller(0).setMilestone(q)
            q[7]+=1.5
            sim.controller(0).addMilestone(q)

Now run the simulation and see what happens.

Sending a trajectory to the controller

Here we'll use a trajectory that's been saved to disk, using the klampt.trajectory module. Unlike the prior example, which used the controller's trajectory queue, we'll send this motion at a high rate to the robot using PID commands. (These override the controller's trajectory queue.)

First, we'll load the trajectory into a class variable by putting these lines at the end of the __init__ method:

        self.traj = trajectory.RobotTrajectory(self.world.robot(0))
        self.traj.load("../Klampt/data/motions/athlete_flex.path")

... and then we will put the following code in the control handler:

        sim = self.sim
        traj = self.traj
        starttime = 2.0
        if sim.getTime() > starttime:
            (q,dq) = (traj.eval(self.sim.getTime()-starttime),traj.deriv(self.sim.getTime()-starttime))
            sim.controller(0).setPIDCommand(q,dq)

That's it! For more information on controlling Python simulations, visit the Custom Controller Tutorial.

Playing God: applying forces and constraining velocities

The robot controller is not able to apply arbitrary forces to its body or the world. This encapsulation is deliberate, because a robot cannot "play God" -- it can only affect its body or the world via its actuators. But it is often useful to generate simulation scenarios by "playing God," and to do so, you must access the SimBody elements that give you direct access to the rigid bodies in the underlying simulator.

The first step in doing so is to access the SimBody out of the Simulator corresponding to the desired object in the WorldModel. To do so, you would call something like this:

body = sim.body(world.robotlink(my_robot_index,my_link_index));
#or...
body = sim.body(world.rigidObject(my_object_index));

To apply forces, you may use the SimBody.applyForceAtPoint function as follows:

body.applyForceAtPoint([fx,fy,fz],[px,py,pz]);

Where the force (fx,fy,fz) and point (px,py,pz) are in world coordinates. You may also call SimBody.applyWrench to apply a force/torque about the center of mass.

Directly controlling the movement of a body (e.g., to move along a predetermined path, or according to a joystick) is possible but takes a few extra steps, because Klamp't by default gives control of the body to the simulator. First, you will need to know the translational and angular velocity along which the body should be moving at each time step. Let us assume you have determined these quantities as (vx,vy,vz) and (wx,wy,wz); both are in world coordinates. Then, you will need to disable dynamic simulation, and during your time step you will need to set the velocities directly as follows:

body.enableDynamics(False)
body.setVelocity((wx,wy,wz),(vx,vy,vz))

Note the angular velocity is provided as the first argument.

Extracting contact forces

For research and evaluation purposes it is often useful to record the contact forces generated by the simulation, and Klamp't provides several functions for doing so. This tutorial also illuminates some of the differences between the indexing and body ID schemes.

The first step in extracting contact feedback is to enable it. Contact feedback is enabled on a per body pair basis; this is primarily done to save a little overhead in computation and memory. Each rigid body in the world, including environment objects and robot links, is given a unique ID, and this ID is used to identify the corresponding body in the simulator. To get the ID of an object in the world you call getID() on it:

terrainid = world.terrain(terrain_index).getID()
objectid = world.rigidObject(object_index).getID()
linkid = world.robot(robot_index).link(link_index).getID()
#equivalent to
linkid = world.robotlink(robot_index,link_index).getID()

IDs are constant throughout the life of the simulation. (NOTE: IDs will change if you add or remove elements from the world -- this is not yet supported in simulation.) We can then just do something like this to enable only collision feedback between the terrain and all links on the robot:

for i in range(world.robot(robot_index).numLinks())
  sim.enableContactFeedback(terrainid,world.robotlink(robot_index,i).getID())

IDs are assigned contiguously, and hence it is possible to just loop through integers ranging from 0 to world.numIDs()-1 to enable all contact pairs (Don't worry, the computational overhead is not that high). This is done frequently, so we have a convenience function

sim.enableContactFeedbackAll()

Let's assume we added the above line to our code. Then, during the simulation loop, we can use the following code to see what objects are in contact. We can also print out the latest force/torque:

contacted=False
for i in range(world.numIDs()):
  for j in range(i+1,world.numIDs()):
    #you could loop over a selective set of id pairs rather than i and j, if you wanted...
    if sim.inContact(i,j):
      if not contacted:
        print "Touching bodies:"
        contacted=True
      f = sim.contactForce(i,j)
      t = sim.contactTorque(i,j)
      print " ",world.getName(i),"-",world.getName(j),"contact force",f,"and torque",t

Even more detailed information about the latest contact points can be retrieved using the sim.getContacts() function. This returns a list of 7-lists, each of which contains the 3D contact point, 3D contact normal, and the friction coefficient. So the following code would print out all contacts between the given objects:

contactlist = sim.getContacts(objectid,linkid)
for c in contactlist:
  print "Contact point",c[0:3],"normal",c[3:6],"friction coefficient",c[6]