Validation of the coherence of C++ and Python implementation.

[jmirabel]

I have done some work on the integration of TOPPRA inside my packages using the C++ API. I have some first results. To go further, I would like to:

• validate that the C++ and Python implementation are coherent (in order to test the C++ implementation) and
• be able to understand the infeasibility, when TOPPRA says a case is not feasible (in order to check that the result is actually correct).

At the moment, I was able to by-pass infeasible situations by increasing some robot limits. I have doubts the situations were truly infeasible but, of course, it is hard to be sure. I dig a bit the issue and ended up with a message from either qpOASES or GLPK which tells me two inequalities could not be met (but they don't tell which one, or I did not find the API to do so).

@hungpham2511 do you have insights on those two points ?

[hungpham2511]

Is the issue you are having related to #112?

Among these return codes, https://github.com/hungpham2511/toppra/blob/5ffa8e1b92c8151e837a2fd4253948860d9bbc38/cpp/src/toppra/algorithm.hpp#L13 are you able to detect when does qpoases/glpk fail?

The Python implementation is quite robust to generic instances. The only issue that I am still not confident in solving is paths with very small derivatives. (#26) Though rescaling the problem like the hotqpoases solver wrapper seems to greatly resolve this issue.

validate that the C++ and Python implementation are coherent (in order to test the C++ implementation) and

This is one of my motivations for developing the Python bindings.

[jmirabel]

Is the issue you are having related to #112?

The result I describe are the result I obtain after solving #112.

are you able to detect when does qpoases/glpk fail?

The return code tells me it fails during the computation of the controllable set 15 over 50 (in reverse order, so the 35th computed controllable set). By turning on verbosity for both solvers, I get

INFO:  Using regularisation as Hessian matrix is not positive definite
INFO:  Iteration 0 ...
INFO:   Stepsize is 9.793074697930415e-19! (idx = 2, isBound = 0, status = 1)
INFO:  Adding to active set: upper constraint's bound no. 2.
INFO:  New bound/constraint is linearly dependent
INFO:  Removing from active set: bound no. 0.
INFO:  Linear independence of active constraint matrix successfully resolved
INFO:  Iteration 1 ...
INFO:   Stepsize is 5.265323502427799e-16! (idx = 5, isBound = 0, status = 1)
INFO:  Adding to active set: upper constraint's bound no. 5.
INFO:  New bound/constraint is linearly dependent
INFO:  Removing from active set: constraint no. 2.
INFO:  Linear independence of active constraint matrix successfully resolved
INFO:  Iteration 2 ...
INFO:   Stepsize is 2.637900176520259e-04! (idx = 56, isBound = 0, status = 1)
INFO:  Adding to active set: upper constraint's bound no. 56.
INFO:  New bound/constraint is linearly dependent
INFO:  Removing from active set: bound no. 1.
INFO:  Linear independence of active constraint matrix successfully resolved
INFO:  Iteration 3 ...
INFO:   Stepsize is 3.049564186702234e-06! (idx = 1, isBound = 1, status = 1)
INFO:  Adding to active set: upper bound no. 1.
INFO:  New bound/constraint is linearly dependent
INFO:  Removing from active set: constraint no. 5.
INFO:  Linear independence of active constraint matrix successfully resolved
INFO:  Iteration 4 ...
INFO:   Stepsize is 1.447392674430747e-01! (idx = 42, isBound = 0, status = 1)
INFO:  Adding to active set: upper constraint's bound no. 42.
INFO:  New bound/constraint is linearly dependent
INFO:  Removing from active set: bound no. 1.
INFO:  Linear independence of active constraint matrix successfully resolved
INFO:  Iteration 5 ...
INFO:   Stepsize is 5.172832503929356e-03! (idx = 1, isBound = 1, status = -1)
INFO:  Adding to active set: lower bound no. 1.
INFO:  New bound/constraint is linearly dependent
INFO:  Removing from active set: constraint no. 56.
INFO:  Linear independence of active constraint matrix successfully resolved
INFO:  Iteration 6 ...
INFO:   Stepsize is 1.142879910232056e-01! (idx = 1, isBound = 1, status = 0)
INFO:  Removing from active set: bound no. 1.
INFO:  Iteration 7 ...
INFO:   Stepsize is 0.000000000000000e+00! (idx = 1, isBound = 1, status = 1)
WARNING:   Stepsize is 0.000000000000000e+00
INFO:  Adding to active set: upper bound no. 1.
INFO:  New bound/constraint is linearly independent
INFO:  Iteration 8 ...
INFO:   Stepsize is 7.109882492217523e-01! (idx = 1, isBound = 0, status = 1)
INFO:  Adding to active set: upper constraint's bound no. 1.
INFO:  New bound/constraint is linearly dependent
INFO:  Removing from active set: bound no. 1.
INFO:  Linear independence of active constraint matrix successfully resolved
INFO:  Iteration 9 ...
INFO:   Stepsize is 7.372681094700471e-01! (idx = 1, isBound = 1, status = -1)
INFO:  Adding to active set: lower bound no. 1.
INFO:  New bound/constraint is linearly dependent
INFO:  Linear independence of active constraint matrix successfully resolved
INFO:  Iteration 9* ...
INFO:   Stepsize is 4.107112628302646e-17! (idx = 1, isBound = 1, status = -1)
INFO:  Adding to active set: lower bound no. 1.
INFO:  New bound/constraint is linearly dependent
INFO:  Linear independence of active constraint matrix successfully resolved
ERROR:  Premature homotopy termination because QP is infeasible
->ERROR:  Initial QP could not be solved due to infeasibility!
[DEBUG]: qpOASES failed. Error code: Initial QP could not be solved due to infeasibility! (37)
[DEBUG]: Fail: controllable, upper problem, idx: 15

for qpOASES and

[DEBUG]: 15, 50
GLPK Simplex Optimizer, v4.65
67 rows, 2 columns, 134 non-zeros
     72: obj =   4.073621207e-10 inf =   8.460e-02 (1)
LP HAS NO PRIMAL FEASIBLE SOLUTION
[DEBUG]: GLPK: problem has no feasible solution
Writing problem data to '/tmp/lp_problem.txt'...
210 lines were written
[DEBUG]: Fail: controllable, upper problem, idx: 15

for GLPK.

validate that the C++ and Python implementation are coherent (in order to test the C++ implementation) and

Do we have a simple mean of generating realistic problem instances, that we could use both in C++ and Python ?

[hungpham2511]

The return code tells me it fails during the computation of the controllable set 15 over 50 (in reverse order, so the 35th computed controllable set). By turning on verbosity for both solvers, I get

It's possible that the instance is indeed uncontrollable. In my experience, I have not faced many numerical issues in computing the controllable sets.

Do we have a simple mean of generating realistic problem instances, that we could use both in C++ and Python ?

What I have in mind now is to solve the same random instances in both Python and C++ and verify that they return the same values. Do you have any suggestions?

[jmirabel]

Do you have any suggestions?

I will propose a code for this. What kind of polynomial interpolation SimplePath does in Python ?

[hungpham2511]

I will propose a code for this. What kind of polynomial interpolation SimplePath does in Python ?

Cubic interpolation. More or less similar to the constructHemite static method.

The result I describe are the result I obtain after solving #112.

Have you implemented the fix for this issue, I see that it is linked with #111

[jmirabel]

I tried this in Python (I know how to do the same thing in C++ except for the transformation from waypoionts to geometric path):

import pinocchio
import numpy as np

def torque_constraint(robot, scale=1.):
    from toppra.constraint import JointTorqueConstraint
    def inv_dyn (q, v, a):
        return pinocchio.rnea(robot.model, robot.data, q, v, a)
    return JointTorqueConstraint (inv_dyn,
        np.vstack([-scale*robot.model.effortLimit,scale*robot.model.effortLimit]).T,
        np.zeros(robot.model.nv))

def joint_velocity_constraint(robot, scale=1.):
    from toppra.constraint import JointVelocityConstraint
    return JointVelocityConstraint(
        np.vstack([-scale*robot.model.velocityLimit,scale*robot.model.velocityLimit]).T
        )

def generate_random_trajectory(robot, npts, maxd):
    from toppra.simplepath import SimplePath

    ts = [0.,]
    qs = [pinocchio.randomConfiguration(robot.model),]
    while len(qs) < npts:
        qlast = qs[-1]
        q = pinocchio.randomConfiguration(robot.model)
        d = pinocchio.distance(robot.model, qlast, q)
        if d > maxd:
            q = pinocchio.interpolate(robot.model, qlast, q, maxd / d)
        qs.append(q)
        ts.append(ts[-1] + pinocchio.distance(robot.model, qlast,q))

    return SimplePath(np.array(ts), np.array(qs))

if __name__ == "__main__":
    from example_robot_data.robots_loader import loadUR
    robot = loadUR(limited=True)
    path = generate_random_trajectory(robot, 50, 0.3)

    constraints = [
            #joint_velocity_constraint(robot),
            torque_constraint(robot),
            ]

    from toppra.algorithm import TOPPRA
    algo = TOPPRA(constraints, path, solver_wrapper='seidel')

I first got:

/home/jmirabel/.local/lib/python3.6/site-packages/toppra/constraint/joint_torque.py in compute_constraint_params(self, path, gridpoints) 83 ) 84 v_zero = np.zeros(path.dof) ---> 85 p = path.eval(gridpoints) 86 ps = path.evald(gridpoints) 87 pss = path.evaldd(gridpoints)

AttributeError: 'SimplePath' object has no attribute 'eval'

I added the missing eval functions and then, I have a size error. The configuration sent to the inv_dyn function is not of the correct size (the velocity and acceleration are of correct size). Am I doing something wrong ?

[jmirabel]

Using SplineInterpolator instead of SimplePath works fine.

[jmirabel]

Using #114 I was able to run both Python and C++ implementation on the same trajectory and I get different result. I will provide a code soon.

[jmirabel]

Note that the issue disappears when I don't use the joint torque constraint.

[hungpham2511]

Using #114 I was able to run both Python and C++ implementation on the same trajectory and I get different result. I will provide a code soon.

In #114 I have not implemented the binding for joint torque constraint. Been thinking how to do that with Pinocchio.