+
+ +
+

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. TODO: explain simulation modes: all=kinematic, robot=kinematic + objects=physical, all=physical TODO: PhysX multi-body mode (motors), gains, friction (gravity compensation?)

+
+

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:

+
+
[1]:
+
+
+
from robotic import ry
+import time
+
+
+
+
+
[2]:
+
+
+
# 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:

+
+
[3]:
+
+
+
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.)

+
+
[4]:
+
+
+
ry.params_clear()
+ry.params_add({'physx/defaultRestitution': 1.})
+ry.params_print()
+
+
+
+
+
+
+
+
+-- ry.cpp:operator():86(0) physx/defaultRestitution: 1
+
+
+

Let’s try again:

+
+
[5]:
+
+
+
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.

+
+
[6]:
+
+
+
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:

+
+
[7]:
+
+
+
from robotic import ry
+import numpy as np
+import time
+
+
+
+
+
[8]:
+
+
+
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()
+
+
+
+
+
[8]:
+
+
+
+
+0
+
+
+
+
[9]:
+
+
+
S = ry.Simulation(C, ry.SimulationEngine.physx, verbose=0)
+
+
+
+
+
[10]:
+
+
+
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()
+
+
+
+
+
[11]:
+
+
+
move_a_bit()
+
+
+
+
+
[12]:
+
+
+
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
+
+
+
+
[13]:
+
+
+
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
+
+
+
+
[14]:
+
+
+
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
+
+
+
+
[15]:
+
+
+
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?
+
+
+
+
[16]:
+
+
+
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:

+
+
[17]:
+
+
+
from robotic import ry
+import time
+
+
+
+
+
[18]:
+
+
+
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.addAttribute('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.addAttribute('restitution', .5+.1*i)
+C.view()
+
+
+
+
+
[18]:
+
+
+
+
+0
+
+
+
+
[19]:
+
+
+
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()
+
+
+
+
+
[20]:
+
+
+
S=0
+C=0
+
+
+
+
+
+
+
+
+-- simulation.cpp:~Simulation:148(0) shutting down Simulation
+
+
+
+
+

Resetting and messing with state

+
+
[21]:
+
+
+
from robotic import ry
+import time
+
+
+
+
+
[22]:
+
+
+
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()
+
+
+
+
+
[22]:
+
+
+
+
+0
+
+
+
+
[23]:
+
+
+
S = ry.Simulation(C, ry.SimulationEngine.physx, 1)
+
+
+
+
+
[24]:
+
+
+
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:

+
+
[25]:
+
+
+
#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()
+
+
+
+
+
[25]:
+
+
+
+
+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:

+
+
[26]:
+
+
+
S.setState(X)
+#updates the simulation window
+
+
+
+
+
[27]:
+
+
+
#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:

+
+
[28]:
+
+
+
S.setState(X0, q0)
+C.view()
+
+
+
+
+
[28]:
+
+
+
+
+0
+
+
+
+
[29]:
+
+
+
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.)

+
+
[30]:
+
+
+
del S
+del C
+
+
+
+
+
+
+
+
+-- simulation.cpp:~Simulation:148(0) shutting down Simulation
+
+
+
+
[ ]:
+
+
+

+
+
+
+
+
+ + +
+