Direct Simulation Interface

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 physical 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:

from robotic import 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()

-- ry.cpp:operator():86(0) python,
message: "this parameter was loaded from 'rai.cfg'",
physx/verbose: 1,
physx/yGravity!,
physx/softBody!,
physx/multiBody,
physx/multiBodyDisableGravity,
physx/jointedBodies!,
physx/angularDamping: 0.1,
physx/defaultFriction: 1,
physx/defaultRestitution: 0.1,
physx/motorKp: 1000,
physx/motorKd: 100

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

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

-- ry.cpp:operator():86(0) physx/defaultRestitution: 1

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()

-- simulation.cpp:~Simulation:148(0) shutting down Simulation

As you can see, “restitution” means bouncing. Below is an example of how to set bounciness and friction per object.

del S
del C
ry.params_clear()

-- simulation.cpp:~Simulation:148(0) shutting down Simulation

Including robots/articulated configurations

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

from robotic import 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()

0

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

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()

move_a_bit()

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

-- simulation.cpp:~Simulation:148(0) shutting down Simulation

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

-- simulation.cpp:~Simulation:148(0) shutting down Simulation

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()

-- simulation.cpp:~Simulation:148(0) shutting down Simulation

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

-- simulation.cpp:~Simulation:148(0) shutting down Simulation
-- WARNING:kin_physx.cpp:prepareLinkShapes:626(-1) computing compound inertia for object frame 'world' -- this should have been done earlier?

del S
del C

-- simulation.cpp:~Simulation:148(0) shutting down Simulation

Friction and bouncing per object

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

from robotic import 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()

0

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

-- simulation.cpp:~Simulation:148(0) shutting down Simulation

Resetting and messing with state

from robotic import 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()

0

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

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()

move_a_bit()

We are in the middle of some action. Let’s swap two blocks (top/bottom). 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()

0

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

S.setState(X)
#updates the simulation window

#recall that the position control starts from zero, leading to a hard PD jerk initially
move_a_bit()

We can also recreate the initial state:

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

0

move_a_bit()

Internally, not only the frame state (pose of all dynamic objects), but also the joint state (motor states/targets) are 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

-- simulation.cpp:~Simulation:148(0) shutting down Simulation