Klamp't Tutorial: Creating a world

In this tutorial we will learn the first step in setting up your simulation--creating and modifying a world. Klampt allows you to create a world in 2 ways: static XML file editing or programmatic creation. These 2 approaches can be used at the same time as well. For a simple,relatively static world setting, editing XML file is usually sufficient. However, if you want to add/delete objects as your simulation goes or say check for collisions with terrian and objects in the existing world before you create a new object, the ability to create world programmatically becomes very handy.

Difficulty: simple

Time: 20 minutes

Not done yet. Please check for the Python API tutorial for now, C++ version should come up soon.

In this tutorial we will be creating a simple world using python API. We will be going through an example step by step to set up a tx90 robot in a world that has coffee mug on a table through XML editing, we will then add a shelf to the world programmactically in the main file.

A World model contains some number of entities, which can be of the type robot, robot link, rigid object, and terrain. Each entity has an index into the list of entities of that type, a unique ID number in the World and could be consisted of one or more bodies where each body has a coordinate frame and usually some attached geometry and an appearance.The 3 main elements-- robot, terrain and objects can be built using robot models and geometry primitives located in Klampt/data folder.

A robot can be understood as articulated and possibly actuated objects containing multiple robot links. You can make your own robot.rob file or use the available robot models located at Klampt/data/robots folder.You should checkout the RobotModel class API and learn how to get more information about the robot model you choose. After selecting a robot, the next step is to set up terrian. Terrian is the enviroment the robot stays in and the purpose of the terrian is to defines the physical boundary of the world. Some example terrian files can be found in Klampt/data/terrians. If you are unsure about when to use terrian and when to use objects, a rule of thumb is that you should set up items you won't directly interact with but will limit your working space as terrians(for example, walls, ground or a table, however, you should not set the table as a terrian if you plan to knock it over, because a terrian cannot be moved). If you do want to interact with certain objects and want the objects to have mass and dynamics accosiated with them like objects in the real physical world do, you should set it up as a "rigidObject". The Terrian and rigidObject files available in Klampt/data folder are mostly basic shape primitives that you can connect and combine to make reasonably realistic looking real-world items. However, you can certainly scan and make your own geometrics(e.g. stl file) or download existing geometrics database and load it to the world for customized appearance. There are four currently supported types of geometry: primitives (GeometricPrimitive), triangle meshes ( TriangleMesh), point clouds ( PointCloud), groups (Group).

To play around with the world and objects in the world please refer to this Python klampt.robotsim Namespace Reference .

Now let us go through an example of editing XML file that sets up a simple world. The code below comes from Klampt/data/tx90cuptable.xml, there are lots of other example scenarios in the data folder you can reference if you are learing how to build a world through XML file editing.

<?xml version="1.0" encoding="UTF-8"?> <world> <robot name="TX90L-pr2" file="robots/tx90pr2.rob" /> <terrain file="terrains/block.off" /> <!-- the table --> <terrain file="terrains/cube.off" scale="0.05 0.05 0.6" translation="0.6 0.25 0.01"> <display color="0.4 0.3 0.2"/> </terrain> <terrain file="terrains/cube.off" scale="0.05 0.05 0.6" translation="0.6 -0.25 0.01"> <display color="0.4 0.3 0.2"/> </terrain> <terrain file="terrains/cube.off" scale="0.05 0.05 0.6" translation="1.1 0.25 0.01"> <display color="0.4 0.3 0.2"/> </terrain> <terrain file="terrains/cube.off" scale="0.05 0.05 0.6" translation="1.1 -0.25 0.01"> <display color="0.4 0.3 0.2"/> </terrain> <terrain file="terrains/cube.off" scale="0.6 0.6 0.02" translation="0.575 -0.275 0.615"> <display color="0.4 0.3 0.2"/> </terrain> <rigidObject name="srimug" file="objects/srimugsmooth.obj" position="0.7 0.1 0.75"> <graspGenerator type="GraspIt" file="objects/solution2.txt" /> </rigidObject> <target position="-0.5 0 0.2" /> <simulation> <object index="0"> <geometry padding="0.005" /> </object> </simulation> </world>

This XML should be rather straight forward to read and easy to understand. It first loads a robot tx90pr2.rob from the data/robots folder and specify its type as "robot". Then it loads "terrains/block.off" as terrian, this is the solid ground the robot is standing on. Then the code uses 5 cubes with different scaling and translation and group and combine them in a single terrian "table". Lastly it loades "objects/srimugsmooth.obj as a rigid object and associated it's grasping with a GraspIt solution, this connection is not neccessary at the moment but it is convenient if you are planning to do some object grasping in the future.

You can open your text editor and type in the following code to visualize the world.

from klampt import *
import sys
import time
from klampt.sim import *
from klampt import vis

if __name__ == "__main__":
    if len(sys.argv)<=1:
        print "USAGE: visualize_world.py [world_file]"
        exit()
    world = WorldModel()
    for fn in sys.argv[1:]:
        res = world.readFile(fn)
        if not res:
            raise RuntimeError("Unable to load model "+fn)
    

    vis.add("world",world)
    vis.show()
    
    t0 = time.time()
    while vis.shown():
        t1 = time.time()
	#update your code here	       
       	time.sleep(max(0.01-(t1-t0),0.001))
       	t0 = t1

Name this file visualize_world.py and in the folder that contain this file and run:

python visualize_world.py your_path_to/data/tx90cuptable.xml

Result should look like this:

Import 2

Programmatic creation of a world

Now that we have covered creation of a world through XML editing, let's do an exmaple of adding objects into the world progrommatically. We will add a cube shelf as terrian to the world by writting a function called "make_shelf":


def make_shelf(world,width,depth,height,wall_thickness=0.005):
	"""Makes a new axis-aligned "shelf" centered at the origin with
	dimensions width x depth x height. Walls have thickness wall_thickness. 
	If mass=inf, then the box is a Terrain, otherwise it's a RigidObject
	with automatically determined inertia.
	"""
	left = Geometry3D()
	right = Geometry3D()
	back = Geometry3D()
	bottom = Geometry3D()
	top = Geometry3D()
	left.loadFile("your_path_to/data/objects/cube.off")
	right.loadFile("your_path_to/data/objects/cube.off")
	back.loadFile("your_path_to/data/objects/cube.off")
	bottom.loadFile("your_path_to/data/objects/cube.off")
	top.loadFile("your_path_to/data/objects/cube.off")
	left.transform([wall_thickness,0,0,0,depth,0,0,0,height],[-width*0.5,-depth*0.5,0])
	right.transform([wall_thickness,0,0,0,depth,0,0,0,height],[width*0.5,-depth*0.5,0])
	back.transform([width,0,0,0,wall_thickness,0,0,0,height],[-width*0.5,depth*0.5,0])
	bottom.transform([width,0,0,0,depth,0,0,0,wall_thickness],[-width*0.5,-depth*0.5,0])
	top.transform([width,0,0,0,depth,0,0,0,wall_thickness],[-width*0.5,-depth*0.5,height-wall_thickness])
	shelfgeom = Geometry3D()
	shelfgeom.setGroup()
	for i,elem in enumerate([left,right,back,bottom,top]):
		g = Geometry3D(elem)
		shelfgeom.setElement(i,g)
	shelf = world.makeTerrain("shelf")
	shelf.geometry().set(shelfgeom)
	shelf.appearance().setColor(0.2,0.6,0.3,1.0)
	return shelf

The code first create the left, right, back, bottom and top pieces of the shelf as Geometry3D. A three-D geometry can either be a reference to a world item's geometry, in which case modifiers change the world item's geometry, or it can be a standalone geometry. For more information on Geometry3D class, refer to Python klampt.robotsim.Geometry3D Namespace Reference .

The next step is to load the 3D geometry, in this case cube.off located in data/objects folder. Followed by setting up scaling and transform for each of them (similar to the steps when editing XML). Each geometry stores a "current" transform, which is automatically updated for world items' geometries. The proximity queries are performed with respect to the transformed geometries.

Lastly, the make_shelf function group the seperate pieces together and combine it as one terrain named "shelf" and assign a color to the shelf.

We now make changes to our visualize_world.py to include the make_shelf function, add shelf parameters and call make_shelf function. To properly use of the code, please change the path to the objects to the correct path on your local computer.

from klampt import *
import sys
import time
from klampt.sim import *
from klampt import vis

shelf_dims = (0.4,0.4,0.3)
shelf_offset_x=0.8
shelf_offset_y = 0.1
shelf_height = 0.65

def make_shelf(world,width,depth,height,wall_thickness=0.005):
	"""Makes a new axis-aligned "shelf" centered at the origin with
	dimensions width x depth x height. Walls have thickness wall_thickness. 
	If mass=inf, then the box is a Terrain, otherwise it's a RigidObject
	with automatically determined inertia.
	"""
	left = Geometry3D()
	right = Geometry3D()
	back = Geometry3D()
	bottom = Geometry3D()
	top = Geometry3D()
	left.loadFile("your_path_to/data/objects/cube.off")
	right.loadFile("your_path_to/data/objects/cube.off")
	back.loadFile("your_path_to/data/objects/cube.off")
	bottom.loadFile("your_path_to/data/objects/cube.off")
	top.loadFile("your_path_to/data/objects/cube.off")
	left.transform([wall_thickness,0,0,0,depth,0,0,0,height],[-width*0.5,-depth*0.5,0])
	right.transform([wall_thickness,0,0,0,depth,0,0,0,height],[width*0.5,-depth*0.5,0])
	back.transform([width,0,0,0,wall_thickness,0,0,0,height],[-width*0.5,depth*0.5,0])
	bottom.transform([width,0,0,0,depth,0,0,0,wall_thickness],[-width*0.5,-depth*0.5,0])
	top.transform([width,0,0,0,depth,0,0,0,wall_thickness],[-width*0.5,-depth*0.5,height-wall_thickness])
	shelfgeom = Geometry3D()
	shelfgeom.setGroup()
	for i,elem in enumerate([left,right,back,bottom,top]):
		g = Geometry3D(elem)
		shelfgeom.setElement(i,g)
	shelf = world.makeTerrain("shelf")
	shelf.geometry().set(shelfgeom)
	shelf.appearance().setColor(0.2,0.6,0.3,1.0)
	return shelf


if __name__ == "__main__":
    if len(sys.argv)<=1:
        print "USAGE: visualize_world.py [world_file]"
        exit()
    world = WorldModel()
    for fn in sys.argv[1:]:
        res = world.readFile(fn)
        if not res:
            raise RuntimeError("Unable to load model "+fn)

    shelf = make_shelf(world,*shelf_dims)
    shelf.geometry().translate((shelf_offset_x,shelf_offset_y,shelf_height))


    vis.add("world",world)
    vis.show()
    
    t0 = time.time()
    while vis.shown():
       	t1 = time.time()
       	time.sleep(max(0.01-(t1-t0),0.001))
       	t0 = t1

Again, go into the folder that contains visualize_world.py file run:

python visualize_world.py your_path_to/data/tx90cuptable.xml

Now on top of what you had in the world, there should be a new shelf lying on the table surface:

Import 3

This wraps up our discussion on the basics of creating a world in Klampt, for more advanced usage, including dynamically creating objects from database and adjust placement and orientation of the objects until collision free, please refer to the functions in main.py fie in IROS2016ManipulationChallenge github repository .