Klamp't Tutorial: Connect a Custom Controller to a Klamp't Simulation

In this tutorial we learn how to connect a user-defined controller to a Klamp't simulation. It covers ROS (Robot Operating System) and raw socket bindings, as well as the direct C++ and Python API.

The Socket, C++, and Python controller support low-level PID commands or torques sent to the robot's motors. The ROS interface supports joint trajectories (via the JointTrajectory ROS message). The C++ and Python APIs also support point-to-point motions and joint trajectories.

Difficulty: intermediate

Time: 10-30 minutes

ROS

This tutorial assumes you are generally familiar with ROS and are comfortable running your own ROS packages. You also must have installed the Klamp't Python package.

ROS controller API

Klamp't comes with a standard Python scripts for translating ROS messages to and from a Klamp't simulation. Specifically, they publish the simulation time to the /clock topic, publishes JointState messages to [ROBOT_NAME]/joint_state, and subscribes to standard JointTrajectory messages from /[ROBOT_NAME]/joint_trajectory. Unless you wish to write your own ROS-to-Klamp't bridge (a tedious but relatively straightforward process), your ROS controller should interact with the robot through these topics.

Controller 1

The general architecture is illustrated above.

Building the Klamp't ROS package

First, the Klamp't ROS package needs to be built so that the scripts can communicate via ROS messages. The folder Klampt/Python/control/klampt_catkin contains a Catkin workspace for all of the Klamp't ROS scripts. You may copy this to your preferred Catkin workspace or use it as is.

If not already set up, run

source /opt/ros/groovy/setup.bash

depending on the version of ros you are using.

Inside the catkin root folder, run:

catkin_make
catkin_make install

Since the module only contains Python scripts, everything should build smoothly.

Running the simulator and controller

To run the simulation, we will need to need to start up several components in a particular order. We will use a simple serialization protocol built into Klamp't that lets controllers communicate with external processes via standard socket connections. This is called the Controller Communication Protocol (CCP). You will perform a four step process: 1) configure the simulated robot to send/receive CCP messages, 2) start the ROS<->CCP relay, 3) start your ROS controller, and 4) run the simulation as desired.

Step 1: To configure the simulator, run SimTest with your desired world file, and then select Windows -> Controller..., and then click the Connect Serial button. (The default values are ok for now, and most importantly, the Rate parameter indicates that it will communicate with ROS at 50Hz.) Leave SimTest running.

Step 2: If ROS is not already running, open up a console window and run roscore. Open another console window and set up the Klamp't robot file via

rosparam set /klampt_robot_file [PATH TO KLAMPT MODEL FILE]

This file will determine the mapping from ROS joint names to Klamp't link names, and is assumed to be the same robot as the one running in SimTest.

You should indicate that simulation time is being used by running

rosparam set /use_sim_time true

Finally, run:

rosrun klampt rosserialcontroller.py

to start the ROS<->CCP relay. If all goes well you will see the console display messages indicating a connection was successful.

Step 3: Run your ROS controller as needed. (You must indicate that simulation time is to be used by setting /use_sim_time to true.)

Step 4: Switch back to SimTest and start the simulation as usual. You are free to pause and resume the simulation, interact with the robot via force application mode, and to save movies. You may not, however, revert the simulation, save/load simulation state, or send commands via the poser.

To stop the simulation, you may close down SimTest and rosserialcontroller.py in any order, using Ctrl+C.

Tip: to avoid having to perform several clicks in Step 1, you may specify that the robot is to use a SerialController in the world XML file. See Klampt/data/tx90serialinput.xml for an example.

Tip: The "rosparam set" lines are only required on the first run.

Tip: if you do want to write a ROS-to-Klamp't bridge for your own robot, the Klampt/Python/control/rosbaxtercontroller.py script gives you an example for how to do so. The controller protocol is described in more detail in the Klamp't Manual and in the Python API documentation.

The C++ API provides a controller interface that is the most performance-oriented. The controller will be compiled code, it avoids translation or repackaging of messages, and it provides direct access to Klamp't planning routines and sensors. The only downside is that Klamp't apps must be recompiled to use the controller (like in SimTest), but you can also make your own simulation app that accepts your controller.

What we'll do is create a new directory with three files, MyController.h, main.cpp, and CMakeLists.txt. Let's start with MyController.h. We'll add a new subclass of RobotController (found in Klampt/Control/Controller.h) and override, at a minimum, the Type method, which provides a name for the controller, and the Update method, which reads from the sensors member and writes to the command member.

Your MyController.h will look something like this:

#include "Controller.h"

class MyController : public RobotController
{
public:
  MyController(Robot& robot) : RobotController(robot) {}
  virtual ~MyController() {}
  virtual const char* Type() const { return "MyController"; }
  virtual void Reset() { 
    //put any initialization code here
    RobotController::Reset(); 
  } 
  virtual void Update(Real dt) {
    //We'll put our code here: read from this->sensors, and write to this->command.
    //See Sensor.h and Command.h for details on these structures
    printf("Whee, the control loop is being called!\n");
    //call the base class's Update method
    RobotController::Update(dt);
  }
};

Your controller code should go into the indicated line in Update. For example, we'll implement a controller that moves a link along a fixed velocity for 1 second:

Vector qcmd,vcmd;
Vector qactual,vactual;
GetCommandedConfig(qcmd);  //convenience function in RobotController
GetCommandedVelocity(vcmd);  //convenience function in RobotController
GetSensedConfig(qactual);  //convenience function in RobotController
GetSensedVelocity(vactual);  //convenience function in RobotController
int link = 7;  //the movement link
if(time >= 1.0 && time < 2.0) {
  Real speed = -1.5;
  vcmd[link] = speed;
}
else
  vcmd[link] = 0;
SetPIDCommand(qcmd,vcmd); //convenience function in RobotController

Note that, internally, the GetSensedX and SetPIDCommand convenience functions read from and write to the sensors and command structures, respectively. If you wish to use more complex sensors, e.g. force sensors, gyroscopes, etc., then you will need to read them out of the RobotController's sensors' member. See the documentation of the Robot Sensors class and the Klampt/Control/Sensors.h file for more information.

Let's now switch to main.cpp. We could run a command-line simulation like in the Klamp't simulation tutorial, but let's make things more interesting by making a GUI. The plan is to implement a new version of the SimTest program that will allow us to use this controller. For simplicity we'll copy our main.cpp file from the GLUI version of SimTest, given in Main/simtest.cpp:

#include "Interface/SimTestGUI.h"
#include 
int main(int argc,const char** argv)
{
  RobotWorld world;
  SimTestBackend backend(&world);
  if(!backend.LoadAndInitSim(argc,argv)) {
    return 1;
  }
  GLUISimTestGUI gui(&backend,&world);
  gui.SetWindowTitle("SimTest");
  gui.Run();
  return 0;
}

(If you really cared about getting all the bells and whistles of Qt, you will need to copy the Main/SimTestQt folder into your own folder and make the subsequent modifications to the main.cpp file.)

Next, we need to make Klamp't aware of the new controller type. Klamp't uses a controller factory that is used to create a controller when its type is specified in the world simulation settings, and you need to register your controller with the factory. This is done as follows. First include your controller's header file:

#include "MyController.h"

Then, add the following line before gui.Run():

RobotControllerFactory::RegisterDefault(new MyController(world.robots[0].robot));

Now we are ready to compile your new simulation program. In CMakeLists.txt, you will

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(MySim main.cpp)
TARGET_LINK_LIBRARIES(MySim ${KLAMPT_LIBRARIES})

If all goes well you should see a new executable named MySim.

We're not done yet. We need to instruct Klamp't to use this controller for a simulation. Let's try the Hubo robot. First copy the world XML file Klampt/data/athlete_plane.xml to a new document, say Klampt/data/athlete_mycontroller.xml. Open it in a text editor.

Under the XML <world> tag , add the lines:

<simulation>
  <robot index="0"> 
    <controller type="MyController" />.
  </robot>  
</simulation>

Now save and close the file. Let's run the simulation

./MySim data/athlete_mycontroller.xml

and see the controller in action! The robot should lift its leg.

Remarks: If you want to include your controller as a default for all Klamp't applications to use, you may do the following. Open up Klampt/Control/Controller.cpp. Go to the definition of RobotControllerFactory::RegisterDefault method, add a #include of your controller's header file, and add the line

Register(new MyController(robot))

Next, navigate to the main Klampt directory and recompile Klamp't and SimTest via the command:

make SimTest

This will also build the Klamp't library and will make your controller available for use in any programs that use Klamp't.

This is only the beginning; there are an infinite number of ways to implement a controller, ranging from simple to highly complex behaviors. For example, for point-to-point motions or motions through multiple keyframes, you may subclass the PolynomialPathController class and use its motion queuing functionality. You are only limited by your imagination!

The Python interface gives you a lot of flexibility for controlling robots. The easiest way to understand its interface is to work through the exercise in Klampt/Python/exercises/control. This tutorial provides some more information about the several control methods available in Python.

Manual control interface

The standard Klamp't simulation loop is given as follows:

import klampt
world = klampt.WorldModel()
world.readFile("my_world_file.xml")
sim = klampt.Simulator(world)
controller = sim.getController(0)
while sim.getTime() < 10:
    #TODO put your control code here
    sim.simulate(0.01)
print "End configuration:",controller.getSensedConfig()

In the indicated TODO line, you should call methods of the controller object, which is of the SimRobotController type.

Sensing: To get the encoder configuration and velocity, you may call controller.getSensedConfig() and controller.getSensedVelocity(). To get other sensors, you may call controller.getSensor(index).getMeasurements() or controller.getNamedSensor(name).getMeasurements().

Low-level control: To send a PID command to the robot, you may call controller.setPIDCommand(qdes,dqdes). To send raw torques, you may call controller.setTorques(t).

Trajectory queuing: To send the robot smoothly to a desired configuration, you may call controller.setMilestone(qdes). To queue up several moves, you can then call controller.addMilestone(qdes2), controller.addMilestone(qdes3), etc. See also the Simulation tutorial for a concreete example.

Tip: You may also observe your controller behavior in real-time by copying the Klampt/Python/demos/gltemplate.py program, and then inserting your control code into the control_loop() method.

Standardized controller.py interface

The Python version of SimTest, Klampt/Python/demos/simtest.py, accepts arbitrary controllers given as Python scripts on the command line. These scripts must contain a make(robot) function which return an object that implements the interface in BaseController (Klampt/Python/control/controller.py.

Most importantly, your controller should implement the following:

  • output(self,**inputs): given a set of named inputs, produce a dictionary of named outputs. The semantics of the inputs and outputs are defined below.
  • advance(self,**inputs): advance by a single time step, performing any necessary changes to the controllerís state. Note: output should NOT change internal state!

The inputs argument to both functions is a dictionary with the following elements:

  • t: the current simulation time
  • dt: the controller time step
  • q: the robotís current sensed configuration
  • dq: the robotís current sensed velocity
  • qcmd: the robotís current commanded configuration
  • dqcmd: the robotís current commanded configuration
  • The names of each sensors in the simulated robot controller, mapped to a list of its measurements.

The return value of output should be a dictionary that contains one of the following combinations of keys, signifying which type of joint control should be used:

  • qcmd: use PI control.
  • qcmd and dqcmd: use PID control.
  • qcmd, dqcmd, and torquecmd: use PID control with feedforward torques.
  • dqcmd and tcmd: perform velocity control with the given actuator velocities, executed for time tcmd.
  • torquecmd: use torque control.

Several existing controllers have been implemented in Python/Klampt/control/controller.py to make the design and composition of controllers a bit easier, e.g., finite state machines, switching controllers, linear controllers, etc.

Examine Python/Klampt/control/trajectory_controller.py to see how a standard trajectory controller is implemented.

The Klamp't socket interface uses a simple serialization protocol, known as the Controller Communication Protocol (CCP), in a request-reply architecture. Using this protocol, you can write your controller in any programming language you want as long as it supports sockets and JSON.

To start up this interface, the overall idea is as follows. First, you configure the simulation so that a robot's controller listens on a port, and then you start your controller program, which connects as a client to that port. As it runs, the simulation will then send a sensor message to the controller, and the controller client should reply with a command message. This process will repeat at the control frequency that was specified when the serial controller was started in the simulation.

Controller 2

The general concept is illustrated above.

A simple example: connecting SimTest to a trajectory controller

Let's start with a built-in example. Starting in the Klampt directory, we'll start up SimTest as follows:

./SimTest data/athlete_plane.xml

From the menu, we'll then select Windows -> Controller..., and then click the Connect Serial button. The simulation is now listening for clients, and once they are connected will connect at 50 Hz (by default, this can be changed via the Rate parameter).

Now open up a new command prompt, and go into the Klampt/Python/control directory. We'll now run the serialcontroller.py program, which by default starts up a trajectory-following controller. We'll give it one of our example trajectories:

python serialcontroller.py ../../data/motions/athlete_flex_opt.path

If all goes well, you'll see a line "Accepted new client on tcp://localhost:3456" printed out in the SimTest console window.

Now run the simulation and watch the trajectory go! When you're done you may quit SimTest using Ctrl+C.

Writing your own controller

The process of writing your own controller is a bit more time-consuming because it requires you to manually write a socket client and parse JSON strings. It is beyond the scope of this tutorial to discuss how to write networking code, so let's assume that you have such a framework available. Follow along by looking over the Python example of the JsonClient class in serialcontroller.py. We'll use it as a template for how to implement your own controller in whatever language you wish.

We'll assume that all the network reading and writing code will be similar to JsonClient. You would implement your custom controller by subclassing JsonClient, overriding onMessage, and adding your functionality into this method. This method would process the sensor message to compute a desired command, and call sendMessage to write a command message as a reply.

Before discussing the content of onMessage, let's take an aside to discuss the protocol. The CCP operates on objects that are encoded via JSON-formatted strings. Since these strings are variable sized, the serial protocol sends a 4-byte length header before the message payload. The reader must parse this header to determine the length of the string for reading.

In JsonClient, this functionality is performed in the handle_read method, which is automatically called by Python whenever there's data available on the client socket (in your implementation you may need to call the POSIX select function or equivalent, or to wait for messages via polling). It then reads a 4 byte string from the socket, parses this into an integer msglen, and reads a string of msglen bytes containing the content of the message. It then parses it into a Python object using the built-in json package.

What will happen is that your controller implementation in onMessage will read sensor data contained in this object, compute the desired joint command, and advance any internal state (see ControllerClient.onMessage). It will then send a command object using sendMessage. As observed in JsonClient.sendMessage, the object should be written to a JSON string, and then the client will send the message back to the server, prepended with a 4-byte header indicating the string length.

In other words, a sensor message will look something like this:

           4bytes                              N bytes
[binary encoding of N | payload string, e.g. "{t:0.53,dt:0.02,q:[...],...}"]

Once you have read the request content string, is now the job of your controller program to correctly parse the string, compute the control, and write the reply content string. As a result, it is easiest to write your program in Python or some other language with built-in JSON support.

Let's return to discussing the CCP objects contained in these messages. There are two objects, represented as a structure (dictionary) of elements. Let's look at these elements in greater detail:

A sensor message is a structure with the following elements:

  • t: the current simulation time
  • dt: the controller time step
  • q: the robot's current sensed configuration
  • dq: the robot's current sensed velocity
  • qcmd: the robot's current commanded configuration
  • dqcmd: the robot's current commanded configuration
  • The names of each sensor in the simulated robot controller, mapped to a list of its measurements.

The configuration and velocity variables q, dq, qcmd, and dqcmd are lists of floating point numbers, one for each DOF of the robot. These DOFs are laid out exactly in the order specified in the Klamp't robot, hence your controller must be aware of this ordering (rather than, say, ROS, which uses named indices). By default, a robot has no extra sensors, but new sensors can be specified in the world XML file (see Klampt/data/hubo_plane.xml for an example of this).

A command message is a structure which contains one of the following combinations of keys, signifying which type of joint control should be used:

  • qcmd: use PI control.
  • qcmd and dqcmd: use PID control.
  • qcmd, dqcmd, and torquecmd: use PID control with feedforward torques.
  • dqcmd and tcmd: perform velocity control with the given actuator velocities, executed for time tcmd.
  • torquecmd: use torque control.

Each command key (except tcmd) must be associated with a list of driver values. Note that these are driver values rather than configuration values; as a result your controller must be aware of which drivers are present in the Klamp't robot (as well as their ordering).

Incidentally, this is exactly the same protocol used in the Python API.

As before, there are an infinite number of ways to implement a controller. You are only limited by your imagination!