Intro: BotOp (Robot Operation) interface

BotOp (=robot operation) defines a very narrow interface to control a real or simulated robot. While in initial years we tried all kinds of other interfaces (ROS-given, operational space control interfaces, controller state machines, etc), this one seems most pragmatic, simple, transparent, and compatible to our research work at the LIS research team. There is no ROS or complex IPC involved, just a few threads (communicating with hardwares) interfaced via BotOp.

The interface essentially provides move methods to set or smoothly overwrite a spline reference for the robot. (Also compliance around the reference can be set.) Gripper methods to operate grippers. And getImage.. methods grab images or point clouds from the camera. That’s basically it.

There might be confusion about whether BotOp and Simulation are the same or similar or what’s different. While they both are “robot simulation interfaces”, they actually play very different roles and that’s important to understand: Simulation is a direct interface to physics engines (by default Nvidia Physx), similar to a gym environment, with a non-threaded step function that you call explicitly. You typically use Simulation to train RL, evaluate controllers, or generate data offline, much faster than real time. In constrast, BotOp interfaces or emulates a real robot, running in real time. The interface methods are not a step function, but the exact same as for controlling the real robot: setter methods for control (setting reference splines, impedances, gripper states) and getter methods to get state information (getImage, get_q, etc). Your code runs in parallel to the real world (or the BotOp emulation) and needs to explicitly sync or get the state information. But yes, under the hood BotOp uses a Simulation as the underlying engine when in simulation mode.

If BotOp is run in simulation mode, the simulation can be run in different modes:

• pure kinematic (no physics for objects)

• a physics simulator with physics for objects but still kinematic robot

• a physics simulator with PD motors for the robot

Sending motion based on IK

We’ll show how to make the robot move to pre-computed joint space poses, e.g. computed via IK. Later we modify this to overwriting the motion reference with high frequency, which essentially realizes MPC-style control.

The first step (also for operating the real robot) is always to load a configuration:

import robotic as ry
import numpy as np
import time

C = ry.Config()
C.addFile(ry.raiPath('../rai-robotModels/scenarios/pandaSingle.g'))
C.view(False, 'this is your workspace data structure C -- NOT THE SIMULTATION')

We open a robot interface in simulation (False). True would directly open communication to one or two pandas (depending no how many are defined in C). The botsim/verbose above leads to the explicit verbosity when creating the simulator interface.

ry.params_add({'botsim/verbose': 2})
bot = ry.BotOp(C, useRealRobot=False)

Note the simulation window, showing that the simulation is running in a thread and the given control reference time.

We define 2 reference poses, q0=home and q1=(2nd joint bend), so that we can move back and forth between them:

qHome = bot.get_qHome()
q0 = qHome.copy()
q1 = q0.copy()
q1[1] = q1[1] + .2
print(q0, q1)

The moveTo is the simplest way to move the robot from current to target. It internally creates a cubic B-spline to the target with optimal timing and follows it. The call is non-blocking. Also, your workspace config C is not kept in sync with the real/sim. If you want to wait till the motion is finished, you need to do manually checking the getTimeToEnd (=time til the end of the given spline reference), and meanwhile staying sync’ed.

bot.moveTo(q1)

while bot.getTimeToEnd()>0:
    bot.sync(C, .1)

The internal spline reference can be appended: As moveTo is non-blocking, you can append several moves like this:

print('timeToEnd:', bot.getTimeToEnd())
bot.moveTo(q0)
print('timeToEnd:', bot.getTimeToEnd())
bot.moveTo(q1)
print('timeToEnd:', bot.getTimeToEnd())
bot.moveTo(q0)

while bot.getTimeToEnd()>0:
    bot.sync(C, .1)

Sending paths from KOMO

The above shows moving towards a single target, where BopTo decides on the timing (see timeCost parameter of moveTo). But we can also send a spline computed, e.g., with path optimization. The example is taken from the KOMO tutorial: we transition smoothly through 4 waypoint:

C.addFrame('way1'). setShape(ry.ST.marker, [.1]) .setPosition([.4, .2, 1.])
C.addFrame('way2'). setShape(ry.ST.marker, [.1]) .setPosition([.4, .2, 1.4])
C.addFrame('way3'). setShape(ry.ST.marker, [.1]) .setPosition([-.4, .2, 1.])
C.addFrame('way4'). setShape(ry.ST.marker, [.1]) .setPosition([-.4, .2, 1.4])
C.view()

komo = ry.KOMO(C, 4, 10, 2, False)
komo.addControlObjective([], 0, 1e-1)
komo.addControlObjective([], 2, 1e0)
komo.addObjective([1], ry.FS.positionDiff, ['l_gripper', 'way1'], ry.OT.eq, [1e1])
komo.addObjective([2], ry.FS.positionDiff, ['l_gripper', 'way2'], ry.OT.eq, [1e1])
komo.addObjective([3], ry.FS.positionDiff, ['l_gripper', 'way3'], ry.OT.eq, [1e1])
komo.addObjective([4], ry.FS.positionDiff, ['l_gripper', 'way4'], ry.OT.eq, [1e1])
komo.addObjective([4], ry.FS.jointState, [], ry.OT.eq, [1e1], [], order=1)

ret = ry.NLP_Solver(komo.nlp(), verbose=0 ) .solve()
print('solver return:', ret)
path = komo.getPath()
print('size of path:', path.shape)
komo.view()

We send this motion with the move method, explicitly specifying the timing to be 4 seconds:

bot.move(path, [4.])

del komo

Reactive control: Overwriting the reference (typical way to run medium frequency RL policies)

Note: RL policies output new commands in every time step, let’s say in 10Hz or so). The following is a default way to run such policies. The example below corresponds to when the policy outputs endeffector space targets. Then the policy outputs joint space targets, the IK can be skipped; then the policy outputs endeffector space deltas, just add them up to become targets. The IK method below is simple – for more robust RL execution it should also check collisions and limits.

BotOp is based on setting a spline reference. This becomes reactive, when we can smoothly overwrite the spline reference at any time. Technically (internally), smoothly overwriting means to take the current dynamic state (pose, velocity) and create a new cubic B-spline with current state as start and given target as end, with optimal timing. (See my B-Spline lecture notes.)

To demonstrate this let’s consider a more involved scenario, where the target is a frame that is randomly moving, and we use repeated IK in each cycle to track it. Let’s first setup the scene:

#this reference frame only appears in your workspace C - not the simulation!
target = C.addFrame('target', 'table')
target.setShape(ry.ST.marker, [.1])
target.setRelativePosition([0., .3, .3])
pos = target.getPosition()
cen = pos.copy()
C.view()

The following defines a very basic Inverse Kinematics method – check the KOMO tutorials for more elaborate formulations. You can use a poseDiff instead of positionDiff, if you want full 6DOF endeffector control. Robust systems should also solve for collisions and limits.

def IK(C, pos):
    q0 = C.getJointState()
    komo = ry.KOMO(C, 1, 1, 0, False) #one phase one time slice problem, with 'delta_t=1', order=0
    komo.addObjective([], ry.FS.jointState, [], ry.OT.sos, [1e-1], q0) #cost: close to 'current state'
    komo.addObjective([], ry.FS.jointState, [], ry.OT.sos, [1e-1], qHome) #cost: close to qHome
    komo.addObjective([], ry.FS.positionDiff, ['l_gripper', 'target'], ry.OT.eq, [1e1]) #constraint: gripper position

    ret = ry.NLP_Solver(komo.nlp(), verbose=0) .solve()

    return [komo.getPath()[0], ret]

The following does not move the robot: We use illustrate random IK solutions using the workspace C. No motion is sent to the real/simulated robot:

for t in range(20):
    time.sleep(.1)
    pos = cen + .98 * (pos-cen) + 0.02 * np.random.randn(3)
    target.setPosition(pos)

    q_target, ret = IK(C, pos)
    print(ret)
    C.setJointState(q_target)
    C.view()

We now generate reative motion by smoothly overwriting the spline reference. Increasing time cost makes it more agressive (penalized total duration of estimated cubic spline).

for t in range(100):
    bot.sync(C, .1) #keep the workspace C sync'ed to real/sim, and idle .1 sec
    pos = cen + .98 * (pos-cen) + 0.02 * np.random.randn(3)
    target.setPosition(pos)

    q_target, ret = IK(C, pos)
    if ret.feasible:
        bot.moveTo(q_target, timeCost=5., overwrite=True)

Aborting motion

Good practise is to always allow a user aborting motion execution. In this example, key ‘q’ will break the loop and call a home() (which is the same as moveTo(qHome, 1., True)

for t in range(5):
    print(t)
    bot.moveTo(q1)
    bot.wait(C) #same as 'loop sync til keypressed or endOfTime', but also raises user window
    if bot.getKeyPressed()==ord('q'):
        print("cancelled")
        break;

    bot.moveTo(q0)
    bot.wait(C)
    if bot.getKeyPressed()==ord('q'):
        print("cancelled")
        break;

bot.home(C)

Gripper operation

Gripper movements also do not block:

bot.gripperMove(ry._left, width=.01, speed=.2)

while not bot.gripperDone(ry._left):
    bot.sync(C, .1)

bot.gripperMove(ry._left, width=.075, speed=1)

while not bot.gripperDone(ry._left):
    bot.sync(C, .1)

bot.sync(C)

Camera & Point Could

BotOp also interfaces basic grabbing of image and depth. In simulation model, the sensor name needs to be a frame name that has camera attributes defined. On the real robot, a realsense camera is directly grabbed.

rgb, depth, points = bot.getImageDepthPcl('cameraWrist')

import matplotlib.pyplot as plt

fig = plt.figure(figsize=(10,5))
axs = fig.subplots(1, 2)
axs[0].imshow(rgb)
axs[1].matshow(depth)
plt.show()

The returned points are a point could, that was directly computed from the depth image and the camera intrinsics. The intrinsics are given by the focal lengths (f_x, f_y) and image center (c_x, c_y). We can also manually compute the point cloud as follows:

fxycxy = bot.getCameraFxycxy("cameraWrist")
points2 = ry.depthImage2PointCloud(depth, fxycxy)
np.linalg.norm(points - points2)

The point cloud is given relative to the camera frame. We can display it by creating a dedicates frame, attached to the camera frame, and setting its (purely visual) shape to be the point cloud:

pclFrame = C.addFrame('pcl', 'cameraWrist')
pclFrame.setPointCloud(points, rgb)
pclFrame.setColor([1.,0.,0.]) #only to see it when overlaying with truth
C.view()

Grasp Test

This is a minimalistic demo for box grasm It hard-codes the grasp using waypoints (-> should be replaced by model-based grasp planning (see komo-3-manipulation), or pcl-based grasp prediction) It uses the PhysX engine behind botop to actually simulate the grasp, using PD gains in the fingers to excert foces The focus of this test is how PhysX responds to setting PD and friction parameters of the grasp To this end, the lift is pretty fast… we want to potentially force a slip

[literally translated from c++ test/21-grasp]

import robotic as ry
import numpy as np

# these are global parameters by which you can influence the friction, grasp force, etc...
ry.params_add({
    'physx/angularDamping': 0.1,
    'physx/defaultFriction': 1.,  #reduce -> slip
    'physx/defaultRestitution': .7, #quit bouncy
    'physx/motorKp': 1000.,
    'physx/motorKd': 100.,
    'physx/gripperKp': 1000., #reduce -> slip
    'physx/gripperKd': 100.,
    'botsim/verbose': 0})

C = ry.Config()
C.addFile(ry.raiPath("../rai-robotModels/scenarios/pandaSingle.g"))

C.addFrame("obj") \
      .setPosition([-.25,.1,.7]) \
      .setShape(ry.ST.ssBox, [.04,.2,.1,.005]) \
      .setColor([1,.5,0]) \
      .setMass(.1) \
      .setContact(True)

way0 = C.addFrame("way0", "obj") .setShape(ry.ST.marker, [.1]) .setRelativePosition([0, 0, .2])
way1 = C.addFrame("way1", "obj") .setShape(ry.ST.marker, [.1]) .setRelativePosition([0, .0, .03])

C.view()

# compute 2 joint space waypoints from these endeff pose waypoints using komo
komo = ry.KOMO()
komo.setConfig(C, True)
komo.setTiming(2., 1, 5., 0)
komo.addControlObjective([], 0, 1e-0)
komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq)
komo.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq)
komo.addObjective([1.], ry.FS.poseDiff, ["l_gripper", "way0"], ry.OT.eq, [1e1])
komo.addObjective([2.], ry.FS.poseDiff, ["l_gripper", "way1"], ry.OT.eq, [1e1])

ret = ry.NLP_Solver() \
    .setProblem(komo.nlp()) \
    .setOptions(stopTolerance=1e-2, verbose=4 ) \
    .solve()
print(ret)
komo.set_viewer(C.get_viewer())
komo.view(False, 'these are the joint space waypoints,\n which are used as control points of the BotOp spline execution')

ways = komo.getPath()

back_ways = np.concatenate([ways[0], C.getJointState()]) .reshape([2, C.getJointDimension()])
print(back_ways)

bot = ry.BotOp(C, useRealRobot=False)
bot.home(C)

# open gripper
bot.gripperMove(ry.ArgWord._left, +1., .5)
bot.wait(C, forKeyPressed=False, forTimeToEnd=False, forGripper=True)

# send a spline for execution, and wait til it's done
bot.move(ways, [2., 3.])
bot.wait(C, forKeyPressed=False, forTimeToEnd=True)

# close gripper
bot.gripperMove(ry.ArgWord._left, .015, .5)
bot.wait(C, forKeyPressed=False, forTimeToEnd=False, forGripper=True)

# send a spline for execution, and wait til it's done
bot.move(back_ways, [.1, .5]) #very fast upward motion!
bot.wait(C, forKeyPressed=False, forTimeToEnd=True)

# open gripper
bot.gripperMove(ry.ArgWord._left, +1., .5)
bot.wait(C, forKeyPressed=False, forTimeToEnd=False, forGripper=True)

Shutdown

You always need to shut down processes (e.g. communication with the real robot) properly. That’s done here by explicitly destroying the objects:

del bot
del C

Parameters

BotOp (and other parts of the rai code) use all kinds of internal parameters that can be configured. The best way to look which parameters actually are used/relevant is to retrospect print the list of parameters have been queried by the code so far. That gives an idea of which global parameters exist:

ry.params_print()

That might tell you a lot about what happend internally.

In the context of BotOp, the parameter botsim/engine can also be set to kin, which would create a simulation without physics where merely the robot moves (and grasped object can be attached/detached). The botsim/verbose above leads to the explicit verbosity when creating the simulator interface.

Parameters can be set in a local file rai.cfg, or manually in python with the following calls – but that need’s to be done BEFORE BotOp is created.

ry.params_add({'botsim/verbose': 2., 'physx/motorKp': 10000., 'physx/motorKd': 1000.})
ry.params_add({'botsim/engine': 'physx'}) #makes a big difference!
ry.params_add({'physx/multibody': True}) #makes a big difference!