Differentiable Features & Collision Evaluation

The 1st tutorial introduced the basic concepts of features. The lecture script section ‘Kinematics’ introduces this a bit more formally.

This tutorial first gives an overview over available features, and then provides more details specifically on collision features.

Recap of evaluating features directly

Let’s first recap how features are directly computed on a configuration - as introduced in the 1st tutorial:

from robotic import ry

C = ry.Config()
C.addFile(ry.raiPath('panda/panda.g'))
q = C.getJointState()

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

feature value: [2.89890247e-01 1.25455202e-16 8.05237266e-01]
Jacobian: [[-1.25455202e-16  4.72237266e-01 -1.15172364e-16 -2.32080381e-01
   0.00000000e+00  4.48170606e-02  0.00000000e+00  0.00000000e+00]
 [ 2.89890247e-01  6.43685653e-17  5.54002326e-01  2.83784184e-16
   1.63424512e-01  2.26026356e-16 -1.38777878e-17  0.00000000e+00]
 [ 0.00000000e+00 -2.89890247e-01 -1.79370329e-16  5.11220138e-01
   0.00000000e+00  2.32670220e-01  0.00000000e+00  0.00000000e+00]]

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

(array([ 0.38205142, -0.70710678,  0.59500984]),
 array([[ 7.07106781e-01,  5.95009840e-01,  3.82051424e-01,
         -5.95009840e-01,  3.82051424e-01, -5.95009840e-01,
         -3.82051424e-01,  0.00000000e+00],
        [ 3.82051424e-01,  8.48324576e-17,  7.07106781e-01,
          2.12081144e-16, -2.94260250e-01,  3.71142002e-16,
         -7.07106781e-01,  0.00000000e+00],
        [ 0.00000000e+00, -3.82051424e-01,  5.95009840e-01,
          3.82051424e-01, -5.95009840e-01,  3.82051424e-01,
         -5.95009840e-01,  0.00000000e+00]]))

The signature of the eval method is

• The feature symbol (FS.<name> in python; FS_<name> in cpp)

• The set of frames it refers to, given as list of frame names

• Optionally: A scale, that can also be a matrix to down-project a feature (see below)

• Optionally: A target, which changes the zero-point of the features (see below)

List of features

Here is a full list of feature symbols:

ry.FS.__members__.keys()

dict_keys(['position', 'positionDiff', 'positionRel', 'quaternion', 'quaternionDiff', 'quaternionRel', 'pose', 'poseDiff', 'poseRel', 'vectorX', 'vectorXDiff', 'vectorXRel', 'vectorY', 'vectorYDiff', 'vectorYRel', 'vectorZ', 'vectorZDiff', 'vectorZRel', 'scalarProductXX', 'scalarProductXY', 'scalarProductXZ', 'scalarProductYX', 'scalarProductYY', 'scalarProductYZ', 'scalarProductZZ', 'gazeAt', 'angularVel', 'accumulatedCollisions', 'jointLimits', 'distance', 'negDistance', 'oppose', 'qItself', 'jointState', 'aboveBox', 'insideBox', 'pairCollision_negScalar', 'pairCollision_vector', 'pairCollision_normal', 'pairCollision_p1', 'pairCollision_p2', 'standingAbove', 'physics', 'contactConstraints', 'energy', 'transAccelerations', 'transVelocities'])

But some of these (esp later ones) are exotic or experimental. The core pre-defined features are the following:

FS	frames	D	description
position	[A]	3	3D position of A in world coordinates
positionDiff	[A,B]	3	difference of 3D positions of A and B in world coordinates
positionRel	[A,B]	3	3D position of A in B-coordinates
quaternion	[A]	4	4D quaternion of A in world coordinatesfootnote[There is ways to handle the invariance w.r.t. quaternion sign properly.]
quaternionDiff	[A,B]	4	…
quaternionRel	[A,B]	4	…
pose	[A]	7	7D pose of A in world coordinates
poseDiff	[A,B]	7	…
poseRel	[A,B]	7	…
vectorX	[A]	3	The x-basis-vector of frame A in world coordinates
vectorXDiff	[A,B]	3	The difference of the above for two frames A and B
vectorXRel	[A,B]	3	The x-basis-vector of frame A in B-coordinates
vectorY…			same as above
scalarProductXX	[A,B]	1	The scalar product of the x-basis-vector of frame A with the x-basis-vector of frame B
scalarProduct…	[A,B]		as above
angularVel	[A]	3	The angular velocity of frame A across two configurations (must be order=1!)
accumulatedCollisions	[]	1	The sum of collision penetrations; when negative/zero, nothing is colliding
jointLimits	[]	1	The sum of joint limit penetrations; when negative/zero, all joint limits are ok
negDistance	[A,B]	1	The NEGATIVE distance between convex meshes A and B, positive for penetration
qItself	[]	n	The configuration joint vector
aboveBox	[A,B]	4	when all negative, A is above (inside support of) the box B
insideBox	[A,B]	6	when all negative, A is inside the box B

Position features

Let’s briefly clarify the difference between position, positionDiff, and positionRel:

• position is the position of a frame A in world coordinates

• positionDiff is the difference (in world coordinates) of position B MINUS position A

• positionRel is the position of frame A in the RELATIVE coordinates of frame B

The best example is if A is a camera: Assume you would like to frame B to be positioned exactly at coordinate (0,0,-.3) in the coordinate frame of A – meaning exactly 30cm centrally in front of the camera – then the following feature would evaluate the error:

C.eval(ry.FS.positionRel, ['gripper', 'panda_joint1'], scale=[1], target=[0, 0, .3])

(array([2.89890247e-01, 1.25455202e-16, 1.72237266e-01]),
 array([[ 0.00000000e+00,  4.72237266e-01, -1.15172364e-16,
         -2.32080381e-01,  0.00000000e+00,  4.48170606e-02,
          0.00000000e+00,  0.00000000e+00],
        [ 0.00000000e+00,  6.43685653e-17,  5.54002326e-01,
          2.83784184e-16,  1.63424512e-01,  2.26026356e-16,
         -1.38777878e-17,  0.00000000e+00],
        [ 0.00000000e+00, -2.89890247e-01, -1.79370329e-16,
          5.11220138e-01,  0.00000000e+00,  2.32670220e-01,
          0.00000000e+00,  0.00000000e+00]]))

Concretely, if you minimize the above feature, the gripper will look exactly(=centrally) at panda_joint1 with 30cm distance.

Scalar product features

The scalarProduct features are also very useful to define relative between frames (e.g. for grasping). For instance, (FS.scalarProductXX, {'handL', 'handR'}, target=[1]) says that the scalar product of the x-axes (e.g. directions of the index finger) of both hands should equal 1, which means they are aligned. And

(FS.scalarProductXY, {'handL', 'handR'})
(FS.scalarProductXZ, {'handL', 'handR'})

says that the the x-axis of handL should be orthogonal (zero scalar product) to the y- and z-axis of handR. So this also describes aligning both x-axes. However, this formulation is much more robust, as it has good error gradients around the optimum.

Scale and target transformation

Let’s explain the scale and target in detail: Formally, specifying a target and scale redefines a feature to become

\[\phi(x) \gets \texttt{scale} \cdot (\phi(x) - \texttt{target})\]

The target needs to be a \(D\)-dim vector and defines the zero-point of the feature.

The scale can be

• a scalar (just a factor),

• a \(D\)-vector (multiplying element-wise to the feature)

• or a matrix.

We can do interesting things when scale is a matrix. For instance, if we only want the \(xy\)-position of a frame returned, we can choose a matrix \(S=\begin{pmatrix}1 & 0 & 0 \\ 0 & 1 & 0\end{pmatrix}\), which multiplies to the 3D feature to return a 2D feature. In code:

C.eval(ry.FS.position, ['gripper'], [[1,0,0],[0,1,0]])

(array([2.89890247e-01, 1.25455202e-16]),
 array([[-1.25455202e-16,  4.72237266e-01, -1.15172364e-16,
         -2.32080381e-01,  0.00000000e+00,  4.48170606e-02,
          0.00000000e+00,  0.00000000e+00],
        [ 2.89890247e-01,  6.43685653e-17,  5.54002326e-01,
          2.83784184e-16,  1.63424512e-01,  2.26026356e-16,
         -1.38777878e-17,  0.00000000e+00]]))

A very useful application is again if we care about a camera looking at a point: If we want frame B to appear centrally in the \(xy\)-plane of frame A, we can mimimize the feature:

C.eval(ry.FS.positionRel, ['gripper', 'panda_joint1'], [[1,0,0],[0,1,0]])

(array([2.89890247e-01, 1.25455202e-16]),
 array([[ 0.00000000e+00,  4.72237266e-01, -1.15172364e-16,
         -2.32080381e-01,  0.00000000e+00,  4.48170606e-02,
          0.00000000e+00,  0.00000000e+00],
        [ 0.00000000e+00,  6.43685653e-17,  5.54002326e-01,
          2.83784184e-16,  1.63424512e-01,  2.26026356e-16,
         -1.38777878e-17,  0.00000000e+00]]))

Collision features

Let’s evaluate the accumulative collision scalar and its Jacobian

coll = C.feature(ry.FS.accumulatedCollisions, [])

C.computeCollisions() #collisions/proxies are not automatically computed on set...State
coll.eval(C)

(array([0.]), array([[0., 0., 0., 0., 0., 0., 0., 0.]]))

Let’s move into collision and redo this

from robotic import ry
C = ry.Config()
C.clear()
C.addFile(ry.raiPath('panda/panda.g'))
C.addFile(ry.raiPath('panda/panda.g'), 'r_')
base_r = C.getFrame('r_panda_base')
base_r.setPosition([.0, .5, .0])

<robotic.ry.Frame at 0x7fa7b82d3ab0>

C.selectJoints(['panda_joint1', 'panda_joint2', 'r_panda_joint1', 'r_panda_joint2'])
C.setJointState([1.,-.8,-1.,-.8])
C.view()

0

The configuration is now in collision. We can evaluate between some specific shape-pairs that show this:

C.eval(ry.FS.negDistance, ['palm', 'r_palm'])

(array([0.13976684]),
 array([[ 0.2021495 , -0.06471866,  0.00841045,  0.55724234]]))

C.eval(ry.FS.negDistance, ['panda_coll7', 'r_panda_coll7'])

(array([0.08287262]),
 array([[ 0.14218262,  0.43479204, -0.14218262,  0.43479204]]))

C.eval(ry.FS.negDistance, ['panda_coll6', 'r_panda_coll6'])

(array([-0.05097286]),
 array([[-0.05482199,  0.48443921, -0.12142734,  0.29574326]]))

However, the above features only return penetration (=negDistance) between specific pairs of shapes. For holistic collision checking we query if any pair of shapes collides. This is called broad phase collision checking. The following does this (calling fcl internally):

C.computeCollisions()
C.getCollisions()

[('r_palm', 'palm', -0.13976683583198263),
 ('r_finger1', 'palm', 0.013576748081966544),
 ('r_finger2', 'palm', 0.014228713904190754),
 ('r_panda_coll6', 'palm', -0.015404140037349565),
 ('r_panda_coll5', 'palm', 0.04525630035427913),
 ('r_finger2', 'r_panda_coll5', 0.13749756426853948),
 ('r_panda_coll7', 'palm', -0.09280264001977123),
 ('r_panda_coll2', 'r_panda_coll5', 0.1945494196968938),
 ('r_panda_coll3', 'palm', 0.36007247820155464),
 ('r_palm', 'r_panda_coll3', 0.35692722195160376),
 ('r_panda_coll7', 'r_panda_coll4', 0.32362431478923487),
 ('panda_coll2', 'panda_coll5', 0.19454941969689368),
 ('panda_coll7', 'panda_coll4', 0.32362431478923515),
 ('r_panda_coll2', 'panda_coll0', 0.28717304075316374),
 ('r_panda_coll0', 'panda_coll1', 0.27280115353885176),
 ('r_panda_coll1', 'panda_coll0', 0.2728011535388518),
 ('r_panda_coll0', 'panda_coll0', 0.19999999999999998),
 ('r_panda_coll0b', 'panda_coll1', 0.22160801784824963),
 ('r_panda_coll0b', 'panda_coll0', 0.13131351929875237),
 ('r_panda_coll0', 'panda_coll2', 0.28717304075316374),
 ('r_panda_coll0b', 'panda_coll2', 0.2638941651174542),
 ('r_panda_coll6', 'panda_coll6', 0.050972864653827),
 ('r_panda_coll6', 'panda_coll7', 0.015315130184475917),
 ('r_panda_coll6', 'finger1', 0.05202028460982694),
 ('r_panda_coll6', 'finger2', 0.03488780676276168),
 ('r_panda_coll5', 'panda_coll6', 0.11164377353037033),
 ('r_panda_coll5', 'panda_coll7', 0.08766336859618301),
 ('r_panda_coll5', 'finger1', 0.12895455840607237),
 ('r_panda_coll5', 'finger2', 0.06147141318949159),
 ('r_panda_coll7', 'panda_coll6', 0.013907106655053708),
 ('r_panda_coll7', 'panda_coll7', -0.08287262318906746),
 ('r_panda_coll7', 'finger1', 0.030647107437714566),
 ('r_panda_coll7', 'finger2', 0.03284520763356755),
 ('r_palm', 'panda_coll6', -0.05737613444861561),
 ('r_finger1', 'panda_coll6', -0.020651551158550177),
 ('r_finger2', 'panda_coll6', 0.112620748583009),
 ('r_palm', 'panda_coll7', -0.1273471133054103),
 ('r_finger1', 'panda_coll7', 0.0011556581521950104),
 ('r_finger2', 'panda_coll7', 0.05638875573522109),
 ('r_palm', 'finger1', -0.02159299051272197),
 ('r_palm', 'finger2', -0.014663617396226986),
 ('r_finger2', 'finger1', 0.03712381290870001),
 ('r_finger2', 'finger2', 0.03836716448878992),
 ('palm', 'panda_coll3', 0.3569272219516041),
 ('r_palm', 'panda_coll3', 0.3884474823677896),
 ('r_panda_coll7', 'panda_coll5', 0.08262699740729021),
 ('r_palm', 'panda_coll5', 0.0151854277723599),
 ('r_finger1', 'panda_coll5', 0.037025559294402516),
 ('r_finger2', 'panda_coll5', 0.16135689397161754)]

C.getTotalPenetration()

0.572477645899696

The last command is useful for what is classically called binary collision check: if totalPenetration is zero, we have no collisions.

Finally, we can get the same information in a differentiable manner, as a feature:

C.eval(ry.FS.accumulatedCollisions, [])

(array([0.57247765]),
 array([[ 0.5810677 ,  1.0999702 , -0.74220228,  1.70678119]]))

Enabling/disabling collisions

In a typical robotic configuration we might have both, visual and (more coarse) collision shapes. Broadphase collision checking should only include the collision shapes. Further, after broadphase collision checking one typically wants to filter out some collisions: shapes of consecutive robot links should usually not be included in collision checking.

In the rai code, shapes have an integer contact parameter to control this, with the following semantics:

• contact = 0 (default) –> the shape never collides (e.g., is a visual)

• contact = 1 –> the shape collides with all other shapes

• contact < -k (some negative number) –> the shape collides with all shapes except for those that are k-th order parents

So, the latter setting is very natural in robot chains: contact=-1 means “not colliding with the parent link”. To be precise, the “k-th order parenthood” means frames between which there are k joints or less.

Note contact needs to be set before the first broadphase collision checking, as it is used in the construction of the underlying collision engine.

del C