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.

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

version: 0.0.23 compile time: Oct 20 2023 14:36:37

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

0

A configuration is essentially a tree (or forrest) of frames. Let’s add a frame and set positions manually. 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()

frame name: first pos: [0.  0.  0.5] quat: [0.95782629 0.28734789 0.         0.        ]

0

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

frame name: second pos: [0.  0.  0.5] quat: [-0.95782629 -0.28734789 -0.         -0.        ]

0

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

joint state: [0.2]

0

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

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])
for t in range(100):
    C.setJointState([np.cos(.1*t)])
    C.view()
    time.sleep(.1)

The following lists all predefined shape types:

ry.ST.__members__.keys()

dict_keys(['none', 'box', 'sphere', 'capsule', 'mesh', 'cylinder', 'marker', 'pointCloud', 'ssCvx', 'ssBox', 'ssCylinder', 'ssBoxElip', 'quad', 'camera', 'sdf'])

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

dict_keys(['hingeX', 'hingeY', 'hingeZ', 'transX', 'transY', 'transZ', 'transXY', 'trans3', 'transXYPhi', 'transYPhi', 'universal', 'rigid', 'quatBall', 'phiTransXY', 'XBall', 'free', 'generic', 'tau'])

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

0

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

0

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

0

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

0

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

[ 0.   -1.    0.   -2.    0.    2.    0.    0.05  0.   -1.    0.   -2.
  0.    2.    0.    0.05]
joints: ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_finger_joint1panda_finger_joint2', 'r_panda_joint1', 'r_panda_joint2', 'r_panda_joint3', 'r_panda_joint4', 'r_panda_joint5', 'r_panda_joint6', 'r_panda_joint7', 'r_panda_finger_joint1r_panda_finger_joint2']
frames: ['panda_base', 'panda_link0', 'panda_link0_0', 'panda_joint1_origin', 'panda_joint1', 'panda_link1', 'panda_link1_0', 'panda_joint2_origin', 'panda_joint2', 'panda_link2', 'panda_link2_0', 'panda_joint3_origin', 'panda_joint3', 'panda_link3', 'panda_link3_0', 'panda_joint4_origin', 'panda_joint4', 'panda_link4', 'panda_link4_0', 'panda_joint5_origin', 'panda_joint5', 'panda_link5', 'panda_link5_0', 'panda_joint6_origin', 'panda_joint6', 'panda_link6', 'panda_link6_0', 'panda_joint7_origin', 'panda_joint7', 'panda_link7', 'panda_link7_0', 'panda_joint8_origin', 'panda_joint8', 'panda_link8', 'panda_hand_joint_origin', 'panda_hand_joint', 'panda_hand', 'panda_hand_0', 'panda_finger_joint1_origin', 'panda_finger_joint2_origin', 'panda_finger_joint1', 'panda_finger_joint2', 'panda_leftfinger', 'panda_rightfinger', 'panda_leftfinger_0', 'panda_rightfinger_0', 'panda_coll0', 'panda_coll0b', 'panda_coll1', 'panda_coll3', 'panda_coll5', 'panda_coll2', 'panda_coll4', 'panda_coll6', 'panda_coll7', 'gripper', 'palm', 'finger1', 'finger2', 'r_panda_base', 'r_panda_link0', 'r_panda_link0_0', 'r_panda_joint1_origin', 'r_panda_joint1', 'r_panda_link1', 'r_panda_link1_0', 'r_panda_joint2_origin', 'r_panda_joint2', 'r_panda_link2', 'r_panda_link2_0', 'r_panda_joint3_origin', 'r_panda_joint3', 'r_panda_link3', 'r_panda_link3_0', 'r_panda_joint4_origin', 'r_panda_joint4', 'r_panda_link4', 'r_panda_link4_0', 'r_panda_joint5_origin', 'r_panda_joint5', 'r_panda_link5', 'r_panda_link5_0', 'r_panda_joint6_origin', 'r_panda_joint6', 'r_panda_link6', 'r_panda_link6_0', 'r_panda_joint7_origin', 'r_panda_joint7', 'r_panda_link7', 'r_panda_link7_0', 'r_panda_joint8_origin', 'r_panda_joint8', 'r_panda_link8', 'r_panda_hand_joint_origin', 'r_panda_hand_joint', 'r_panda_hand', 'r_panda_hand_0', 'r_panda_finger_joint1_origin', 'r_panda_finger_joint2_origin', 'r_panda_finger_joint1', 'r_panda_finger_joint2', 'r_panda_leftfinger', 'r_panda_rightfinger', 'r_panda_leftfinger_0', 'r_panda_rightfinger_0', 'r_panda_coll0', 'r_panda_coll0b', 'r_panda_coll1', 'r_panda_coll3', 'r_panda_coll5', 'r_panda_coll2', 'r_panda_coll4', 'r_panda_coll6', 'r_panda_coll7', 'r_gripper', 'r_palm', 'r_finger1', 'r_finger2']

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)

Features: computing geometric properties

For every frame we can query its pose:

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

gripper pos: [0.22972161 0.63675584 0.81163634]
gripper quat: [-0.79663237 -0.24376386  0.46490269  0.29970242]
gripper rot: [[ 0.3880879   0.25085235 -0.8868263 ]
 [-0.70415824  0.70151528 -0.10971544]
 [ 0.59459982  0.66704528  0.44888934]]

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 directly often; but defining differentiable features is 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)

position of gripper: [0.1938467  0.05571584 0.92707862]
Jacobian: [[-5.57158418e-02  5.93961135e-01 -1.59973178e-02 -3.74233985e-01
   3.40893538e-03 -3.55845920e-02  4.33680869e-18  0.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00  0.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 1.93846702e-01 -1.18141540e-02  6.14899795e-01  3.09760548e-02
   1.46708625e-01  3.69133387e-02  5.55111512e-17  0.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00  0.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00 -1.92700374e-01 -5.24486684e-02  4.31994181e-01
  -2.28855824e-02  2.31333401e-01  5.20417043e-18  0.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00  0.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00  0.00000000e+00]]

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

(array([-0.40236503]),
 array([[-2.42147163e-02,  9.94071777e-03,  5.57838017e-01,
         -2.81537448e-02, -4.43005956e-02, -9.59797410e-05,
          1.82288149e-16, -0.00000000e+00,  5.00697618e-03,
         -1.37520471e-02, -5.44630216e-01,  1.79043783e-02,
          7.34786608e-02,  1.94588018e-02, -1.88400330e-17,
         -0.00000000e+00]]))

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

(array([ 0.14746986, -0.58349898,  0.79861228]),
 array([[ 0.58349898,  0.79845435,  0.29047621, -0.73479687,  0.50459046,
         -0.69895086, -0.17492902,  0.        ,  0.        ,  0.        ,
          0.        ,  0.        ,  0.        ,  0.        ,  0.        ,
          0.        ],
        [ 0.14746986, -0.01588162,  0.77300206, -0.01285582, -0.4321508 ,
          0.04105352, -0.81009853,  0.        ,  0.        ,  0.        ,
          0.        ,  0.        ,  0.        ,  0.        ,  0.        ,
          0.        ],
        [ 0.        , -0.15904446,  0.51114844,  0.12629287, -0.40892364,
          0.159062  , -0.55958932,  0.        ,  0.        ,  0.        ,
          0.        ,  0.        ,  0.        ,  0.        ,  0.        ,
          0.        ]]))

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, qw,qx,qy,qz). 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-freedoms (DOFs). If the configuration has in total n DOFs, the joint state is a n-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()

0

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)

frame state:  [[ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  3.33000000e-01  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  3.33000000e-01  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  3.33000000e-01  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  3.33000000e-01  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  3.33000000e-01  7.07106781e-01
  -7.07106781e-01  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  3.33000000e-01  6.20544581e-01
  -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]
 [ 0.00000000e+00  0.00000000e+00  3.33000000e-01  6.20544581e-01
  -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]
 [ 0.00000000e+00  0.00000000e+00  3.33000000e-01  6.20544581e-01
  -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]
 [-2.65904831e-01 -1.05249143e-16  5.03735529e-01  8.77582562e-01
   0.00000000e+00 -4.79425539e-01  0.00000000e+00]
 [-2.65904831e-01 -1.05249143e-16  5.03735529e-01  8.77582562e-01
   0.00000000e+00 -4.79425539e-01  0.00000000e+00]
 [-2.65904831e-01 -1.05249143e-16  5.03735529e-01  8.77582562e-01
   0.00000000e+00 -4.79425539e-01  0.00000000e+00]
 [-2.65904831e-01 -1.05249143e-16  5.03735529e-01  8.77582562e-01
   0.00000000e+00 -4.79425539e-01  0.00000000e+00]
 [-2.21329891e-01 -1.05249143e-16  5.73156885e-01  6.20544581e-01
   6.20544581e-01 -3.39005049e-01  3.39005049e-01]
 [-2.21329891e-01 -1.05249143e-16  5.73156885e-01  6.20544581e-01
   6.20544581e-01  3.39005049e-01 -3.39005049e-01]
 [-2.21329891e-01 -1.05249143e-16  5.73156885e-01  6.20544581e-01
   6.20544581e-01  3.39005049e-01 -3.39005049e-01]
 [-2.21329891e-01 -1.05249143e-16  5.73156885e-01  6.20544581e-01
   6.20544581e-01  3.39005049e-01 -3.39005049e-01]
 [ 5.72200270e-02  2.67785794e-16  8.50054327e-01  8.77582562e-01
   0.00000000e+00  4.79425539e-01  0.00000000e+00]
 [ 5.72200270e-02  2.67785794e-16  8.50054327e-01  8.77582562e-01
   0.00000000e+00  4.79425539e-01  0.00000000e+00]
 [ 5.72200270e-02  2.67785794e-16  8.50054327e-01  8.77582562e-01
   0.00000000e+00  4.79425539e-01  0.00000000e+00]
 [ 5.72200270e-02  2.67785794e-16  8.50054327e-01  8.77582562e-01
   0.00000000e+00  4.79425539e-01  0.00000000e+00]
 [ 5.72200270e-02  2.67785794e-16  8.50054327e-01  6.20544581e-01
   6.20544581e-01  3.39005049e-01 -3.39005049e-01]
 [ 5.72200270e-02  2.67785794e-16  8.50054327e-01  6.20544581e-01
   6.20544581e-01 -3.39005049e-01  3.39005049e-01]
 [ 5.72200270e-02  2.67785794e-16  8.50054327e-01  6.20544581e-01
   6.20544581e-01 -3.39005049e-01  3.39005049e-01]
 [ 5.72200270e-02  2.67785794e-16  8.50054327e-01  6.20544581e-01
   6.20544581e-01 -3.39005049e-01  3.39005049e-01]
 [ 1.04766630e-01  2.67785794e-16  9.24103773e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.04766630e-01  2.67785794e-16  9.24103773e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.04766630e-01  2.67785794e-16  9.24103773e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.04766630e-01  2.67785794e-16  9.24103773e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.94804025e-01  2.67785794e-16  8.66291427e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.94804025e-01  2.67785794e-16  8.66291427e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.94804025e-01  2.67785794e-16  8.66291427e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.94804025e-01  2.67785794e-16  8.66291427e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 1.94804025e-01  2.67785794e-16  8.66291427e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 1.94804025e-01  2.67785794e-16  8.66291427e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 1.94804025e-01  2.67785794e-16  8.66291427e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.43945931e-01  2.71027645e-16  8.34737772e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.43945931e-01  2.71027645e-16  8.34737772e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.63048480e-01 -3.53553800e-02  8.64488229e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.24843382e-01  3.53553800e-02  8.04987314e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.63048480e-01 -3.53553800e-02  8.64488229e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.24843382e-01  3.53553800e-02  8.04987314e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.63048480e-01 -3.53553800e-02  8.64488229e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.24843382e-01  3.53553800e-02  8.04987314e-01 -4.42931549e-01
   3.35835838e-01 -8.10780761e-01  1.83467955e-01]
 [-4.00000000e-02  0.00000000e+00  3.00000000e-02  7.07106781e-01
   0.00000000e+00  7.07106781e-01  0.00000000e+00]
 [-2.00000000e-01 -1.20000000e-01  0.00000000e+00  7.07106781e-01
   0.00000000e+00  7.07106781e-01  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  1.83000000e-01  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [-1.39684183e-01 -1.05249143e-16  4.22690183e-01  8.77582562e-01
   0.00000000e+00 -4.79425539e-01  0.00000000e+00]
 [-1.11074170e-01  2.00000000e-02  7.41993865e-01  8.77582562e-01
   0.00000000e+00  4.79425539e-01  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  3.33000000e-01  6.20544581e-01
  -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]
 [-2.21329891e-01 -1.05249143e-16  5.73156885e-01  6.20544581e-01
   6.20544581e-01  3.39005049e-01 -3.39005049e-01]
 [ 5.72200270e-02  4.00000000e-02  8.50054327e-01  6.20544581e-01
   6.20544581e-01 -3.39005049e-01  3.39005049e-01]
 [ 1.13181340e-01  2.67785794e-16  9.18700750e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 2.89890247e-01  2.67785794e-16  8.05237266e-01 -8.10780567e-01
  -1.83468211e-01  4.42931442e-01  3.35836307e-01]
 [ 1.94804025e-01  2.67785794e-16  8.66291427e-01 -4.43577140e-01
   7.03040009e-01  5.50671700e-01  7.57281032e-02]
 [ 3.03197392e-01 -5.51543927e-02  8.62237905e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.43597439e-01  5.51543927e-02  7.69416477e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 0.00000000e+00  5.00000000e-01  0.00000000e+00  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  5.00000000e-01  0.00000000e+00  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  5.00000000e-01  0.00000000e+00  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  5.00000000e-01  3.33000000e-01  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  5.00000000e-01  3.33000000e-01  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  5.00000000e-01  3.33000000e-01  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  5.00000000e-01  3.33000000e-01  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  5.00000000e-01  3.33000000e-01  7.07106781e-01
  -7.07106781e-01  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  5.00000000e-01  3.33000000e-01  6.20544581e-01
  -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]
 [ 0.00000000e+00  5.00000000e-01  3.33000000e-01  6.20544581e-01
  -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]
 [ 0.00000000e+00  5.00000000e-01  3.33000000e-01  6.20544581e-01
  -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]
 [-2.65904831e-01  5.00000000e-01  5.03735529e-01  8.77582562e-01
   0.00000000e+00 -4.79425539e-01  0.00000000e+00]
 [-2.65904831e-01  5.00000000e-01  5.03735529e-01  8.77582562e-01
   0.00000000e+00 -4.79425539e-01  0.00000000e+00]
 [-2.65904831e-01  5.00000000e-01  5.03735529e-01  8.77582562e-01
   0.00000000e+00 -4.79425539e-01  0.00000000e+00]
 [-2.65904831e-01  5.00000000e-01  5.03735529e-01  8.77582562e-01
   0.00000000e+00 -4.79425539e-01  0.00000000e+00]
 [-2.21329891e-01  5.00000000e-01  5.73156885e-01  6.20544581e-01
   6.20544581e-01 -3.39005049e-01  3.39005049e-01]
 [-2.21329891e-01  5.00000000e-01  5.73156885e-01  6.20544581e-01
   6.20544581e-01  3.39005049e-01 -3.39005049e-01]
 [-2.21329891e-01  5.00000000e-01  5.73156885e-01  6.20544581e-01
   6.20544581e-01  3.39005049e-01 -3.39005049e-01]
 [-2.21329891e-01  5.00000000e-01  5.73156885e-01  6.20544581e-01
   6.20544581e-01  3.39005049e-01 -3.39005049e-01]
 [ 5.72200270e-02  5.00000000e-01  8.50054327e-01  8.77582562e-01
   0.00000000e+00  4.79425539e-01  0.00000000e+00]
 [ 5.72200270e-02  5.00000000e-01  8.50054327e-01  8.77582562e-01
   0.00000000e+00  4.79425539e-01  0.00000000e+00]
 [ 5.72200270e-02  5.00000000e-01  8.50054327e-01  8.77582562e-01
   0.00000000e+00  4.79425539e-01  0.00000000e+00]
 [ 5.72200270e-02  5.00000000e-01  8.50054327e-01  8.77582562e-01
   0.00000000e+00  4.79425539e-01  0.00000000e+00]
 [ 5.72200270e-02  5.00000000e-01  8.50054327e-01  6.20544581e-01
   6.20544581e-01  3.39005049e-01 -3.39005049e-01]
 [ 5.72200270e-02  5.00000000e-01  8.50054327e-01  6.20544581e-01
   6.20544581e-01 -3.39005049e-01  3.39005049e-01]
 [ 5.72200270e-02  5.00000000e-01  8.50054327e-01  6.20544581e-01
   6.20544581e-01 -3.39005049e-01  3.39005049e-01]
 [ 5.72200270e-02  5.00000000e-01  8.50054327e-01  6.20544581e-01
   6.20544581e-01 -3.39005049e-01  3.39005049e-01]
 [ 1.04766630e-01  5.00000000e-01  9.24103773e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.04766630e-01  5.00000000e-01  9.24103773e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.04766630e-01  5.00000000e-01  9.24103773e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.04766630e-01  5.00000000e-01  9.24103773e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.94804025e-01  5.00000000e-01  8.66291427e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.94804025e-01  5.00000000e-01  8.66291427e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.94804025e-01  5.00000000e-01  8.66291427e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 1.94804025e-01  5.00000000e-01  8.66291427e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 1.94804025e-01  5.00000000e-01  8.66291427e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 1.94804025e-01  5.00000000e-01  8.66291427e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 1.94804025e-01  5.00000000e-01  8.66291427e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.43945931e-01  5.00000000e-01  8.34737772e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.43945931e-01  5.00000000e-01  8.34737772e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.63048480e-01  4.64644620e-01  8.64488229e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.24843382e-01  5.35355380e-01  8.04987314e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.63048480e-01  4.64644620e-01  8.64488229e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.24843382e-01  5.35355380e-01  8.04987314e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.63048480e-01  4.64644620e-01  8.64488229e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.24843382e-01  5.35355380e-01  8.04987314e-01 -4.42931549e-01
   3.35835838e-01 -8.10780761e-01  1.83467955e-01]
 [-4.00000000e-02  5.00000000e-01  3.00000000e-02  7.07106781e-01
   0.00000000e+00  7.07106781e-01  0.00000000e+00]
 [-2.00000000e-01  3.80000000e-01  0.00000000e+00  7.07106781e-01
   0.00000000e+00  7.07106781e-01  0.00000000e+00]
 [ 0.00000000e+00  5.00000000e-01  1.83000000e-01  1.00000000e+00
   0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [-1.39684183e-01  5.00000000e-01  4.22690183e-01  8.77582562e-01
   0.00000000e+00 -4.79425539e-01  0.00000000e+00]
 [-1.11074170e-01  5.20000000e-01  7.41993865e-01  8.77582562e-01
   0.00000000e+00  4.79425539e-01  0.00000000e+00]
 [ 0.00000000e+00  5.00000000e-01  3.33000000e-01  6.20544581e-01
  -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]
 [-2.21329891e-01  5.00000000e-01  5.73156885e-01  6.20544581e-01
   6.20544581e-01  3.39005049e-01 -3.39005049e-01]
 [ 5.72200270e-02  5.40000000e-01  8.50054327e-01  6.20544581e-01
   6.20544581e-01 -3.39005049e-01  3.39005049e-01]
 [ 1.13181340e-01  5.00000000e-01  9.18700750e-01  0.00000000e+00
   8.77582562e-01  0.00000000e+00  4.79425539e-01]
 [ 2.89890247e-01  5.00000000e-01  8.05237266e-01 -8.10780567e-01
  -1.83468211e-01  4.42931442e-01  3.35836307e-01]
 [ 1.94804025e-01  5.00000000e-01  8.66291427e-01 -4.43577140e-01
   7.03040009e-01  5.50671700e-01  7.57281032e-02]
 [ 3.03197392e-01  4.44845607e-01  8.62237905e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]
 [ 2.43597439e-01  5.55154393e-01  7.69416477e-01  1.83467955e-01
   8.10780761e-01  3.35835838e-01  4.42931549e-01]]

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

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

0

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

0

## 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_joint1'])
print('joint state: ', C.getJointState())
print('joint names: ', C.getJointNames() )

joint state:  [0.]
joint names:  ['panda_joint1']

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

joint state:  [ 0.      -1.00001  0.      -2.00001  0.       2.00001  0.       0.05
  0.      -1.00001  0.      -2.00001  0.       2.00001  0.       0.05   ]
joint names:  ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_finger_joint1panda_finger_joint2', 'r_panda_joint1', 'r_panda_joint2', 'r_panda_joint3', 'r_panda_joint4', 'r_panda_joint5', 'r_panda_joint6', 'r_panda_joint7', 'r_panda_finger_joint1r_panda_finger_joint2']

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

0

For user interaction it is often useful to wait for a keypress (making view a blocking call):

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

pressed key: 113 q

You can also get a screenshot:

img = C.view_getRgb()
print(type(img), img.shape)

<class 'numpy.ndarray'> (400, 400, 3)

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

And release everything, including closing the view

del C