Intro: Configurations

A configuration is essentially a tree of (coordinate) frames, where each frame can have an associated shape, joint, inertia, etc. This tutorial introduces first steps to creating & loading configurations in rai, setting/getting the joint and frame state, computing features, and handling the view window.

Please see the Robot Kinematics and Dynamics Essentials lecture notes for some background.

Building a configuration from scratch

One typically loads robot configurations from a file. But to learn the concepts, let’s first build a configuration from scratch by adding frames and setting properties such as pose, shape and parentship of frames.

The following creates an empty configuration, and opens a view window for it.

Tip: Make the view window appear “Always On Top” (right click on the window bar).

import robotic as ry
import numpy as np
import time
print('robotic package version:', ry.__version__, ry.compiled())

C = ry.Config()
C.view()

The C.view() command opened a window that shows an empty configuration. It is still responsive (as the underlying glfw event loop is running in a different thread).

Next we declare it a marker frame, which means it has no real shape, but is displayed using its base vectors in the view:

C.clear()
f = C.addFrame(name='first')
f.setShape(type=ry.ST.marker, size=[.3])
f.setPosition([0.,0.,.5])
f.setQuaternion([1., .3, .0, .0]) # more about quaternions below
print('frame name:', f.name, 'pos:', f.getPosition(), 'quat:', f.getQuaternion())
C.view()

Let’s add a second frame, but a bit larget, with the first as parent, and with a hinge joint:

f = C.addFrame(name='second', parent='first')
f.setJoint(ry.JT.hingeX)
f.setShape(type=ry.ST.marker, size=[.4])
print('frame name:', f.name, 'pos:', f.getPosition(), 'quat:', f.getQuaternion())
C.view()

Note that the getPosition and getQuaternion methods understood this is a child frame of the above (i.e., computed forward kinematics). Since we now have a configuration with a joint, we can articulate it:

q = C.getJointState()
q[0] = q[0] + .2
C.setJointState(q)
print('joint state:', q)
C.view()

For illustration, let’s add various other child frames with shapes to the ‘second’ frame and animate it with a trivial loop:

# Add new frames: a box, a ball and a capsule
C.addFrame('ball', 'second') .setShape(ry.ST.sphere, [.1]) .setColor([.7,.0,.0]) .setRelativePosition([-.3,.0,.2])
C.addFrame('box', 'second') .setShape(ry.ST.ssBox, [.3,.2,.1,.02]) .setColor([.0,.7,.0]) .setRelativePosition([.0,.0,.2])
C.addFrame('capsule', 'second') .setShape(ry.ST.capsule, [.3, .05]) .setColor([.0,.0,.7]) .setRelativePosition([.3,.0,.2])

# Articulate the new configuration by moving the hinge joint
for t in range(50):
    C.setJointState([np.cos(.1*t)])
    C.view()
    time.sleep(.1)

The following lists all predefined shape types:

ry.ST.__members__.keys()

For the primitive shapes, the size specifies them (box: 3 widths, sphere: radius, capsule: [length radius], cylinder: [length radius], marker: [axes lengths]). The ssBox is a sphere-swept-Box with a 4D size: [3 widths, corner radius], which is used a lot. Also mesh shapes can be set manually from python (setMesh) by directly giving vertex and triangle arrays, or loaded from a file (setMeshFile). setConvexMesh means sphere-swept convex mesh, which is specified by a mesh and 1D size (radius), and also a very useful shape type. Other methods to set shapes include: setImplicitSurface (for a given 3D tensor, using Marching Cube to find the iso-surface mesh), setTensorShape (a 3D tensor with densities, displayed as transparent density), setPointCloud (optionally with normals), and setLines.

Let’s also briefly list the possible joint types:

ry.JT.__members__.keys()

Please see the Robot Kinematics and Dynamics Essentials for background on such joints (=parameterized transformations). The quatBall perhaps deserves special mentioning: it realizes a ball joint by introducing the 4D quaternion as joint state - this sometimes requires special attention. But the library implements correct Jacobians (accounting also for implicit normalization) w.r.t. the quaternion parameterization. The free joint is a 7D joint with 3D position and quatBall.

There is also a rigid joint, which simply means no DOF and seems fully redundant: having no joint or a rigid joint is the same. However, there is a semantic difference. A child frame without joint is really considered to be a part of the parent: The parent could be a link with multiple shapes (e.g. convex parts) or masses associated, but it is semantically one big link. When a frame has a joint (including a rigid joint), it is not considered part of its parent. That semantics makes a difference, e.g. in picking or walking scenarios, where objects may kinematically switch their parent.

Specifying transformations: Quaternions & turtle strings

See the Quaternions lecture note for background. Note that ry.Quaternion implements exactly the methods described in that note for converting between rotation representations. We use the (w,x,y,z) convention for quaternions. Whenever you set a quaternion manually, it is internally normalized. So setting a quaterniong (1,1,0,0) is totally fine and means 90-degrees around x:

C.clear()
C.addFrame('A') .setShape(ry.ST.marker, [.3]) .setPosition([0.,0.,.5]) .setQuaternion([1., 1., .0, .0])
C.view()

But specifying transformations using translation and quaternion is not always intuitive as a human. Therefore, we also allow for a turtle string notation, namely a string that chains little interpretable ‘translate’ and ‘rotate’ commands to define a transformation, as in the old turtle language. Here is an example:

C.clear()
f = C.addFrame('A') .setShape(ry.ST.marker, [.3])
f.setPoseByText('t(0 0 .5) d(30 1 0 0) t(0 0 .5) d(30 1 0 0)') #t=translate, #d=rotate-by-degrees around axis
C.view()

Here is a specification of the possible commands:

  t(x y z)       # translation by (x,y,z)
  q(q0 q1 q2 q3) # rotation by a quaternion
  r(r x y z)     # rotation by `r` _radians_ around the axis (x,y,z)
  d(d x y z)     # rotation by `d` _degrees_ around the axis (x,y,z)
  E(r p y)       # rotation by roll-pitch-yaw Euler angles

Loading existing configurations

You will usually load predefined configurations from file, and then perhaps edit and combine them manually. The rai package has several robot models predefined and installed in the raiPath. These are yaml files listing the frames (for historical reasons they have the .g-file extension (see Notes:Yaml-Graph Files). See the Import/Edit tutorial, e.g. for loading URDF or Mujoco.

C.clear()
C.addFile(ry.raiPath('panda/panda.g'))
C.view()

Let’s add a second panda, but to avoid frame name conflicts we prefix all frame names. That’s often important when importing multiple models into a single configuration. We can move the second robot simply by redefining the position of its base frame.

C.addFile(ry.raiPath('panda/panda.g'), 'r_')
base_r = C.getFrame('r_panda_base')
base_r.setPosition([.0, .5, .0])
C.view()

These models have several joints. We can get the joint state of the full configuration:

print(C.getJointState())
print('joints:', C.getJointNames())
print('frames:', C.getFrameNames())

Let’s animate - without respect for limits or collisions!

q0 = C.getJointState()
for t in range(20):
    q = q0 + .1*np.random.randn(q0.shape[0])
    C.setJointState(q)
    C.view()
    time.sleep(.2)

Looping through frames and retrieving frame information

Sometimes we want to loop through all frames (e.g. to export a configuration). In python, we can get the specs of a frame as a dict, and set/add attributes of a frame. And we can write all frames in a fresh .yml file:

C = ry.Config()
C.addFile(ry.raiPath('panda/panda.g'))
q0 = C.getJointState()
C.view(False)

for f in C.getFrames():
    print(f.name, f.asDict()) #info returns all attributes, similar to what is defined in the .g-files
    #see also all the f.get... methods

# setting attributes:
f = C.getFrames()[0]
f.setAttributes({'myvalue': 12.345, 'myname': 'Hari Seldon'})
print(f.asDict())

# writing a configuration to a file:
with open("z.yml", "w") as fil:
    fil.write(C.write())

Features: computing geometric properties

For every frame we can query its pose:

f = C.getFrame('gripper')
print('gripper pos:', f.getPosition())
print('gripper quat:', f.getQuaternion())
print('gripper rot:', f.getRotationMatrix())

The above provides basic forward kinematics: After setJointState you can query the pose of any frame.

However, there is a more general way to query features, i.e. properties of the configuration in a differentiable manner. Features are defined and explained in more detail in the Features tutorial.

Features allow you not only to query the value, but also the Jacobian. In practise, one rarely queries features directly (as below). But they are essential to define constraints and objectives for inverse kinematics and path optimization problems (see KOMO tutorial). Here an example of how to query them directly:

[y,J] = C.eval(ry.FS.position, ['gripper'])
print('position of gripper:', y, '\nJacobian:', J)

# negative(!) distance between two convex shapes (or origin of marker)
C.eval(ry.FS.negDistance, ['panda_coll7', 'panda_coll7'])

# the x-axis of the given frame in world coordinates
C.eval(ry.FS.vectorX, ['gripper'])

See the next Features tutorial on all the things you can compute over configurations, and that can be used to formulate optimization problems.

Joint and Frame State

A configuration is a tree of \(n\) frames. Every frame has a pose (position & quaternion), which is represented as a 7D vector \((x,y,z, q_w,q_x,q_y,q_z)^T\). The frame state is the \(n\times 7\) matrix, where the \(i\)-th row is the pose of the \(i\)-th frame.

A configuration also defines joints, which means that the relative transfromation from a parent to a child frame is parameterized by degrees-of-freedom (DOFs). If the configuration has in total \(d\) DOFs, the joint state is a \(d\)-dimensional vector.

Setting the joint state implies computing all relative transformations, and then forward chaining all transformations to compute all frame poses. So setting the joint state also sets the frame state.

Setting the frame state allows you to set frame poses that are inconsistent/impossible w.r.t. the joints! Setting the frame state implies computing all relative transformations from the frame poses, and then assigning the joint state to the projection onto the actual DOFs

C.setJointState(q0)
C.view()

The frame state is a \(n\times 7\) matrix, which contains for all of \(n\) frames the 7D pose. A pose is stored as [p_x, p_y, p_z, q_w, q_x, q_y, q_z], with position p and quaternion q.

X0 = C.getFrameState()
print('frame state: ', X0)

Let’s do a very questionable thing: adding .1 to all numbers in the frame matrix!

X = X0 + .1
C.setFrameState(X)
C.view()

That totally broke the original design of the robot! Setting global frame states overwrites the relative transformations between frames! (Also, the rows of X have non-normalized quaternions. But these are normalized when setting the frame state.)

Let’s reset:

C.setFrameState(X0)
C.view()

## Selecting joints

Often one would like to choose which joints are actually active, that is, which joints are referred to in q. This allows one to sub-select joints and work only with projections of the full dof state. This changes the joint state dimensionality, including ordering of entries in q. The frame state is not affected by such a selection of active joints.

C.selectJoints(['panda_joint1', 'panda_joint2'])
print('joint state: ', C.getJointState())
print('joint names: ', C.getJointNames() )

C.selectJoints([], notThose=True)
print('joint state: ', C.getJointState())
print('joint names: ', C.getJointNames() )

Look closely at the above: We suddenly have 8 joints! (When loading the configuration we only saw 7.) The reason is that the last joint panda_finger_joint1 is (when loaded) non-selected (inactive).

However, even if a joint is non-selected, one can still query or set them directly, by extra arguments of setJointState, as shown below. However, a really important implication of selecting (or de-selecting) joints is w.r.t. the Jacobian: The Jacobians are always returns only w.r.t. the currently selected joints. (And also optimization problems (in KOMO) are only w.r.t. the currently selected joints).

Let’s demonstrate explicit access:

C.clear()
C.addFile(ry.raiPath('panda/panda.g'))
q = C.getJointState()
print('joint state shape: ', q.shape)

C.setJointState([.04], ['panda_finger_joint1']) #closing the fingers to a 2cm gap (panda_finger_joint2 is a mimic joint)
C.setJointState([.1,.1], ['panda_joint2','panda_joint4']) #stretching these joints
C.view()

View interaction and releasing objects

You can close and re-open the view window

C.view_close()

# things are still there
C.view(pause=False, message='this is a message')

For user interaction it is often useful to wait for a keypress (i.e., by making view a blocking call via pause=True):

pressed_key = C.view(pause=True, message='press some key!')
print('pressed key:', pressed_key, chr(pressed_key))

As you can see above, .view() also returnes the ID of key that was pressed, which can be helpful for debugging purposes.

To visualize your configuration outside of the viewer, you can also get a screenshot:

img = C.viewer().getRgb()
print(type(img), img.shape)

import matplotlib.pyplot as plt
plt.imshow(img)
plt.show()

Cleanup

Don’t forget to release everything, including closing the view

del C