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 Script:Configurations of configurations and degrees-of-freedom.
Building a configuration from scratch
Let’s first build a configuration from scratch by adding frames and setting properties such as pose, shape and parentship of frames.
[ ]:
import robotic as ry
import numpy as np
import time
print('version:', ry.__version__, ry.compiled())
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).
[ ]:
C = ry.Config()
C.view()
The C.view()
command will open a window that shows an empty configuration.
A configuration is essentially a tree (or forest) of frames. You usually add models from files, but let’s do it manually here. 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 with 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])
f.setColor([1,0,0])
print('frame name:', f.name, 'pos:', f.getPosition(), 'quat:', f.getQuaternion())
C.view()
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([1.,.5,.0]) .setRelativePosition([-.3,.0,.2])
C.addFrame('box', 'second') .setShape(ry.ST.ssBox, [.3,.2,.1,.02]) .setColor([.5,1.,.0]) .setRelativePosition([.0,.0,.2])
C.addFrame('capsule', 'second') .setShape(ry.ST.capsule, [.3, .05]) .setColor([.0,1.,.5]) .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 most of these, the size
fully 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. ssCvx
means sphere-swept convex mesh, which is specified by a mesh and 1D size (radius), and also a very useful shape type.
Let’s also briefly list the possible joint types:
[ ]:
ry.JT.__members__.keys()
Please see the Script:Kinematics on the basics of such joints (=parameterized transformations). The quatBall
perhaps deserves special explanation: it realizes a ball joint by introducing the 4D quaternion as joint state - this sometimes requires special attention. But the rai code 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 rarely
makes a difference, but it does in certain picking or walking scenarios, where objects, endeffectors or feet attach to other objects via a rigid joint.
Specifying transformations: Quaternions & turtle strings
See the Script:Transformations on background on 3D transformations, rotations, and quaternions. 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 to specify 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.setPose('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
. They typically have the .g
-file extension (see Script:gFiles on this file format, which is a yaml-conform graph file). Converting from URDF to configuration files is possible (see advanced tutorial).
[ ]:
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 then also 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! (E.g., the fingers go out of limits.)
[ ]:
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
[ ]:
C = ry.Config()
C.clear()
C.addFile(ry.raiPath('panda/panda.g'))
q0 = C.getJointState()
C.view(False)
for f in C.getFrames():
print(f.name, f.info()) #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.addAttributes({'myvalue': 12.345, 'myname': 'Hari Seldon'})
print(f.info())
# writing a configuration to a file:
with open("z.g", "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 configuration frame.
However, there is a more general way to query features (see Script:Features), i.e. properties of the configuration in a differentiable manner. You might not use this often; but it is important to understand as these differentiable features are the foundation of how optimization problems are formulated, which you will need a lot.
Here are some example features to evaluate:
[ ]:
[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'])
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! 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 configuration 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() )
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.view_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
[ ]: