KOMO: Reporting & Explaining Convergence

When designing motion problems using KOMO, it is really important to check feasibility and – if the point of convergence is infeasible or different to what you expected – know how to introspect the result. The key here is get information about which constraints were actually active, what their constraint violation is, and “how much each constraint is pulling” at the point of convergence. I recently figured a way to quantify the latter (inspecting the gradients the constraints contribute to the underlying Lagrangian), which really “explains” the point of convergence as good as it gets.

You should have done the first KOMO tutorial first.

Problem specs reporting

Let’s first define a simple feasible KOMO problem (same as in the 1st tutorial), solve it, and get info on convergence:

from robotic import ry
import numpy as np
import time

C = ry.Config()
C.addFile(ry.raiPath('scenarios/pandaSingle.g'))
C.addFrame('box') \
    .setPosition([-.25,.1,1.]) \
    .setShape(ry.ST.ssBox, size=[.06,.06,.06,.005]) \
    .setColor([1,.5,0]) \
    .setContact(True)
C.view()

0

komo = ry.KOMO(C, 1, 1, 0, False) # minimalisitc IK problem: just 1 slice
komo.addControlObjective([], 0, 1e-1) # basic homing (0th order) objective on joint angles q
komo.addObjective([], ry.FS.positionDiff, ['l_gripper', 'box'], ry.OT.eq, [1e1]);

Even before we run optimization, we can a report containing the komo specs and list of all objectives.

komo.report(True)

{'KOMO_specs': {'x_dim': 7,
  'dual_dim': 0,
  'T': 1,
  'k': 0,
  'phases': 1.0,
  'slicesPerPhase': 1,
  'enableCollisions': False,
  'tau': 1.0,
  '#slices': 1,
  '#totalDOF': 7,
  '#frames': 66,
  '#pathQueries:': 7},
 'objectives': {'_F_qItself/0-#14': {'order': 0.0,
   'type': 'sos',
   'scale': [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
   'err': 0.004141463188074276,
   'times': [],
   'slices': [0.0, 0.0]},
  '__F_PositionDiff/0-l_gripper-box': {'order': 0.0,
   'type': 'eq',
   'scale': [10.0],
   'err': 0.0018838214351309113,
   'times': [],
   'slices': [0.0, 0.0]}},
 'totals': {'sos': 0.004141463188074276,
  'ineq': 0.0,
  'eq': 0.0018838214351309113,
  'f': 0.0}}

Constraints error reporting

After optimization, the report will contain errors (i.e. constraint violations, or sqr costs) for all objects. In the current case, all constraint violations are very small:

ret = ry.NLP_Solver(komo.nlp(), verbose=0 ) .solve()
q = komo.getPath()
C.setJointState(q[0])
C.view()
print(ret)

{ time: 0.001226, evals: 6, done: 1, feasible: 1, sos: 0.00414146, f: 0, ineq: 0, eq: 0.00188382 }

komo.report()

{'_F_qItself/0-#14': {'order': 0.0,
  'type': 'sos',
  'scale': [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
  'err': 0.004141463188074276,
  'times': [],
  'slices': [0.0, 0.0]},
 '__F_PositionDiff/0-l_gripper-box': {'order': 0.0,
  'type': 'eq',
  'scale': [10.0],
  'err': 0.0018838214351309113,
  'times': [],
  'slices': [0.0, 0.0]},
 'totals': {'sos': 0.004141463188074276,
  'ineq': 0.0,
  'eq': 0.0018838214351309113,
  'f': 0.0}}

Let’s make the problem infeasible by moving the target out of reach and repeating optimization:

C.getFrame('box') .setPosition([.8, .8, 1.])
C.view()

0

komo.updateRootObjects(C)
solver = ry.NLP_Solver(komo.nlp(), verbose=0 )
ret = solver.solve()
q = komo.getPath()
C.setJointState(q[0])
C.view()
print(ret)

{ time: 0.013015, evals: 120, done: 1, feasible: 0, sos: 0.0702195, f: 0, ineq: 0, eq: 4.60313 }

komo.report()

{'_F_qItself/0-#14': {'order': 0.0,
  'type': 'sos',
  'scale': [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
  'err': 0.07021953478317369,
  'times': [],
  'slices': [0.0, 0.0]},
 '__F_PositionDiff/0-l_gripper-box': {'order': 0.0,
  'type': 'eq',
  'scale': [10.0],
  'err': 4.603126581420996,
  'times': [],
  'slices': [0.0, 0.0]},
 'totals': {'sos': 0.07021953478317369,
  'ineq': 0.0,
  'eq': 4.603126581420996,
  'f': 0.0}}

We clearly see that the positionDiff eq-objective is violated (err includes the scale as factor, so err=4.6 means 0.46 meters error).

Lagrange gradients reporting

Seeing only objective errors sometimes does not really explain what is the issues. So instead we can let the solver report what the lagrange gradients w.r.t. each feature are:

solver.reportLagrangeGradients(komo.getFeatureNames())

{'__F_PositionDiff/0-l_gripper-box': {'err': 4.603126581420996,
  'grad': 1996.3797729703783,
  'type': 'eq'},
 '_F_qItself/0-#14': {'err': 0.07021953478317369,
  'grad': 0.10441937164903314,
  'type': 'sos'}}

Note that this list is always sorted, starting with largest gradients first. For complex problems, this report can be rather insightful. Sometimes one explicitly sees which objectives fight against each other in the Lagrangian by both having similar large gradient sizes (typically with opposing directions, which the report does not show; also a matrix with “angles between gradients” can be computed).

Plotting constraint errors over time for paths

The above only considers a simple IK problem. Let’s look at a path problem (we take the same 4-waypoint problem from the 1st tutorial):

C = ry.Config()
C.addFile(ry.raiPath('scenarios/pandaSingle.g'))
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])
q0 = C.getJointState()
C.view()

0

C.setJointState(q0)
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)

solver = ry.NLP_Solver(komo.nlp(), verbose=0 )
ret = solver.solve()
print(ret)
q = komo.getPath()

for t in range(q.shape[0]):
    C.setJointState(q[t])
    C.view(False, f'waypoint {t}')
    time.sleep(.1)

{ time: 0.041055, evals: 25, done: 1, feasible: 1, sos: 16.5173, f: 0, ineq: 0, eq: 0.000774498 }

Using plotOverTime in the report method now plots the constraint violations (or sqr costs) over phase, using gnuplot. (You need to have gnuplot installed on your Ubuntu, using sudo apt install gnuplot.)

Here, the only interesting (non-zero) signal is from the 2nd-order control costs, which are the acceleration costs. This is a typical acceperation cost profile for optimal paths between waypoints.

R = komo.report(False, plotOverTime=True)

Finally, the largrange gradients show that the control costs and the final velocity constraints contribute a lot, but each waypoint also contributes a significant gradient that explains the point of convergence. (If you’d increase control cost scaling, the waypoint Lagrange parameters and their reported gradients would equally increase.)

solver.reportLagrangeGradients(komo.getFeatureNames())

{'_F_qItself/2-#14': {'err': 16.271221226873042,
  'grad': 6007.932041855862,
  'type': 'sos'},
 '_F_qItself/1-#14': {'err': 3.2991984627583903e-05,
  'grad': 215.44352203086478,
  'type': 'eq'},
 '__F_PositionDiff/0-l_gripper-way3': {'err': 0.00010220589093085097,
  'grad': 38.292276583455326,
  'type': 'eq'},
 '__F_PositionDiff/0-l_gripper-way1': {'err': 0.00022613176104052668,
  'grad': 36.94496585387665,
  'type': 'eq'},
 '__F_PositionDiff/0-l_gripper-way2': {'err': 0.00016647615025650042,
  'grad': 31.041675044147212,
  'type': 'eq'},
 '__F_PositionDiff/0-l_gripper-way4': {'err': 0.0002466926005761927,
  'grad': 22.00293286910548,
  'type': 'eq'},
 '_F_qItself/0-#14': {'err': 0.24605158167255367,
  'grad': 1.2106791822071845,
  'type': 'sos'}}

Test the following: Change the timing of the 3rd waypoint to “[2]” as well, requiring the robot to be at way2 and way3 at the same time – which clearly is infeasible. Check the solver return and lagrange gradients.

del komo
del C