Extension - Simulation: Low-level stepping interface & gym environments

BotOp is a narrow control interface to a real or simulated robot, which is also real time and threaded (as for a real robot). However, sometimes we need a more low-level interface to a physics simulator, e.g. to implement a Reinforcement Learning environment.

Minimalistic example

Let’s first create the smallest possible example: A dropping ball. All we have to do is create a configuration with a ball, create an “attached” simulation, and step it:

import robotic as ry
import time

# minimalistic configuration
C = ry.Config()
C.addFrame('ball') .setShape(ry.ST.sphere, [.2]) .setMass(.1) .setPosition([0,0,1])
C.view()

S = ry.Simulation(C, ry.SimulationEngine.physx, verbose=0)

tau=.01
for i in range(200):
    time.sleep(tau)
    S.step([], tau,  ry.ControlMode.none)
    C.view()

Note that a Simulation is directly operating on the given configuration C. E.g., when you step the simulation, it changes the state of C. In that sense, the simulation is rigidly associated/attached to C. (This is different to BotOp, where the real robot (or simulation) is separate from C and you have to explicitly sync them. It’s also different from providing C in a constructor of KOMO, as KOMO creates it’s own copies of configurations. The simulation class doesn’t copy C, it operates directly on it.)

There are a number of (global) parameters used when creating a simulation. As always, we can see which parameters were queried by params_print:

ry.params_print()

And params_add allows you to set parameters. (Actually append, which is why we first need to clear.)

As an example, let’s increase restitution, which is bounciness.

ry.params_clear()
ry.params_add({'physx/defaultRestitution': .9})
ry.params_print()

Let’s try again:

del S
C.getFrame('ball') .setPosition([0,0,1])
S = ry.Simulation(C, ry.SimulationEngine.physx, verbose=0)

tau=.01
for i in range(200):
    time.sleep(tau)
    S.step([], tau,  ry.ControlMode.none)
    C.view()

Below is an example of how to set bounciness and friction per object.

The display window might not show a smooth video. That’s because C.view does not force a rendering of the current frame, but only triggers it if the display thread is idle. Depending on the window manager that might lead to gaps in the display.

del S
del C
ry.params_clear()

Including robots/articulated configurations

Let’s first give a basic example, pushing a block, which uses default settings and a predefined robot:

import robotic as ry
import numpy as np
import time

C = ry.Config()
C.addFile(ry.raiPath('scenarios/pandaSingle.g'))
C.view(False)

C.addFrame('box') \
    .setShape(ry.ST.ssBox, size=[.1,.1,.1,.005]) .setColor([1,.5,0]) \
    .setPosition([.1,.35,.9])

C.addFrame('stick', 'l_gripper') \
    .setShape(ry.ST.capsule, size=[.3,.02]) .setColor([.5,1,0]) \
    .setRelativePosition([0,0,-.13])

C.setJointState([.0], ['l_panda_joint2']) #only cosmetics
C.setJointState([.02], ['l_panda_finger_joint1']) #only cosmetics

q0 = C.getJointState()
X0 = C.getFrameState()

C.view()

In this scene, the orange box is above the table and will fall, and the robot will execute a trivial turn to the right that should push the box. In the following, we will create a simulation engine for this scene several times, with different parameters. The first will actually lead to a frequent error…

def move_a_bit(T=100):
    tau = .01
    q = q0.copy()
    for i in range(T):
        time.sleep(tau)
        q[0] = q[0] - tau*1.
        S.step(q, tau,  ry.ControlMode.position)
        C.view()

S = ry.Simulation(C, ry.SimulationEngine.physx, verbose=0)
move_a_bit()

As you can see, the robot actually moved a bit and started to push, but got blocked by the box hanging in the air instead of dropping. The error: the box was created as a static object. You can verify this by setting verbose=2, which gives valuable information on the instantiation of the scene in the physical simulation.

First rule: Only objects that have a non-zero mass become a dynamic object in the physics engine.

So let’s add mass, and destroy and recreate the physics engine again:

del S
C.setFrameState(X0)
C.getFrame('box') .setMass(.1)
S = ry.Simulation(C, ry.SimulationEngine.physx, verbose=0)
move_a_bit()

That worked. But let’s try another fail: setting a transparent color:

del S
C.setFrameState(X0)
C.getFrame('box') .setColor([1,1,0,.5])
S = ry.Simulation(C, ry.SimulationEngine.physx, verbose=0)
move_a_bit()

Second rule: Transparent objects or shapes are not at all considered for creation in the physx engine. (The rational is that I use transparent colors only for helper shapes, not real objects.)

And another fail: Let’s attach the box to the table:

del S
C.setFrameState(X0)
C.getFrame('box') .setColor([1,1,0]) .setParent(C.getFrame('table')) .setRelativePosition([.1,.35,.3])
S = ry.Simulation(C, ry.SimulationEngine.physx, verbose=0)
move_a_bit()

What happened here is more surprising and involved: The box is now a child frame of the table. Direct child frames (where there is no joint between child and parent) are really considered to be part of the parent. Also recursively. So the box is now just a sub-frame of the table, which is a sub-frame of the ‘world’ frame. (In general, this is how composed objects are represented, with one parent frame that may have multiple child frames carrying shapes and/or inertia.) In addition, the box as a child is giving mass to it’s root frame (here the ‘world’ frame) – therefore, while ‘world’ (with table shape) was previously a static object in physx, it now becomes dynamic, with two attached shapes. Again, setting verbose=2 reports this.

Finally, the 2nd argument allows a choice of underlying physics engine, namely PhysX (by Nvidia), bullet (as in pybullet), and ‘kinematic’. The latter is actually no physics engine at all, but just kinematically executes articulated motion. Therefore, objects do not move at all – and also collision is not avoided:

del S
C.setFrameState(X0)
C.getFrame('box') .unLink()
S = ry.Simulation(C, ry.SimulationEngine.kinematic, verbose=0)
move_a_bit()

del S
del C

Friction and bouncing per object

Let’s build a configuration manually to test varying friction and bouncing (=restitution) per object:

import robotic as ry
import time

C = ry.Config()
C.addFrame('table') .setShape(ry.ST.ssBox, [2., 1., .1, .02]) .setColor([.3]) \
    .setPosition([0,0,.3]) .setQuaternion([1,-.25,0,0])

for i in range(10):
    f = C.addFrame(f'block_{i}')
    f.setShape(ry.ST.ssBox, [.1,.2,.1,.02]) .setColor([1,.1*i,1-.1*i])
    f.setPosition([.7 - .15*i,-.2,1.])
    f.setMass(1.)
    f.setAttribute('friction', .05*i)

for i in range(10):
    f = C.addFrame(f'ball_{i}')
    f.setShape(ry.ST.sphere, [.05]) .setColor([1,.1*i,1-.1*i])
    f.setPosition([.7 - .15*i,.2,1.])
    f.setMass(.2)
    f.setAttribute('restitution', .5+.1*i)
C.view()

S = ry.Simulation(C, ry.SimulationEngine.physx, verbose=0)

tau=.01
for i in range(200):
    time.sleep(tau)
    S.step([], tau,  ry.ControlMode.none)
    C.view()

S=0
C=0

Resetting and messing with state

Resetting state without recreating the engine can save a lot of time. The interface supports (re)set and overwrite the physis engine state arbitrarily. Let’s create a basic stack of blocks and a minimalistic finger:

import robotic as ry
import time

C = ry.Config()

for i in range(5):
    f = C.addFrame(f'block_{i}')
    f.setShape(ry.ST.ssBox, [.2,.3,.2,.02]) .setColor([1,.2*i,1-.2*i])
    f.setPosition([0,0, .25*(i+1)])
    f.setMass(.1)

C.addFrame('base') .setPosition([1., 0, .5]) .addAttributes({'multibody': True})

C.addFrame('finger', 'base') .setShape(ry.ST.ssBox, [.3, .1, .1, .02]) .setColor([.9]) \
    .setMass(.1) \
    .setJoint(ry.JT.transX)

q0 = C.getJointState()
X0 = C.getFrameState()

C.view()

The blocks will fall, and we realize a simple forward motion of the finger:

def move_a_bit():
    tau = .01
    q = q0.copy()
    for i in range(100):
        time.sleep(tau)
        q[0] = q[0] - tau*1.
        S.step(q, tau,  ry.ControlMode.position)
        C.view()

S = ry.Simulation(C, ry.SimulationEngine.physx, 1)
move_a_bit()

Here we set verbose=1 to have an additional simulation display; which will help understanding the following:

We are in the middle of some action. Let’s swap two blocks (top/bottom) by manipulating the configuration’s frame state. And also displace the bottom one:

#swap two blocks
X = C.getFrameState()
A = X[0,:].copy()
X[0,:] = X[4,:]
X[4,:] = A
X[4,1] = .2
C.setFrameState(X)
C.view()

If you compare the simulation display with the configuration display, you see the difference. Now, the simulation interface allows you to push the configuration back into the physics simulation, overwriting the physical state:

S.setState(X)
#updates also the simulation window

If we continue with the simulation, all behaves ‘normal’. We tricked the physics engine to adopt the previous pose of the orange block to be the new one of the pink, without it getting broken.

move_a_bit()

If the finger motion surprised you: We called the ‘move_a_bit’ method, which always starts with setting the finger reference position (of the underlying PD actuation) back to q0. (We could have reset the finger to its original position, by calling S.setState(X, q0) instead, see below.)

Instead of messing with the state, we can also reset it to the original frame state (including the joint state). And the result should be a simulation (approx.) idential to the first time we simulated:

S.setState(X0, q0)
C.view()

move_a_bit()

Internally, not only the frame state (pose of all dynamic objects), but also the joint state (motor states/targets) are now overwritten in the physics simulator. The optional arguments to setState allow you to also set the current frame and joint velocities. (Frame velocities is a n-by-2-by-3 tensor, as you get it from getState.)

del S
del C