Timing statistics of IK

francesco-romano commented 6 years ago

In this issue we collect timing statistics regarding the inverse kinematics with the aim of finding (and removing) potential bottlenecks.

The IK is configured in the following way:

Robot: iCub
Eq. Constraints: left foot (full 3D : dof=6), right foot (full 3D : dof=6), CoM position (: dof=3)
In. Constraints: none
Joint limits
Parametrization: RPY
Cost: Joints regularisation, Torso orientation
Controlled joints: 15 (legs and torso)

In general IK returns in less than 10 iterations, a part some sporadic cases.

Here is a very long run (longest)

Number of Iterations....: 133

                                   (scaled)                 (unscaled)
Objective...............:   1.4621712534352573e-03    1.4621712534352573e-03
Dual infeasibility......:   5.5420759928266284e-05    5.5420759928266284e-05
Constraint violation....:   1.9349939706092556e-09    1.9349939706092556e-09
Complementarity.........:   2.5659855140747359e-10    2.5659855140747359e-10
Overall NLP error.......:   5.5420759928266284e-05    5.5420759928266284e-05

Number of objective function evaluations             = 525
Number of objective gradient evaluations             = 128
Number of equality constraint evaluations            = 525
Number of inequality constraint evaluations          = 525
Number of equality constraint Jacobian evaluations   = 135
Number of inequality constraint Jacobian evaluations = 135
Number of Lagrangian Hessian evaluations             = 0
Total CPU secs in IPOPT (w/o function evaluations)   =      0.068
Total CPU secs in NLP function evaluations           =      0.040

Timing Statistics:

OverallAlgorithm....................:      0.108 (sys:      0.032 wall:      0.134)
 PrintProblemStatistics.............:      0.000 (sys:      0.000 wall:      0.000)
 InitializeIterates.................:      0.000 (sys:      0.000 wall:      0.000)
 UpdateHessian......................:      0.004 (sys:      0.000 wall:      0.003)
 OutputIteration....................:      0.000 (sys:      0.000 wall:      0.001)
 UpdateBarrierParameter.............:      0.012 (sys:      0.012 wall:      0.013)
 ComputeSearchDirection.............:      0.028 (sys:      0.012 wall:      0.037)
 ComputeAcceptableTrialPoint........:      0.044 (sys:      0.008 wall:      0.069)
 AcceptTrialPoint...................:      0.000 (sys:      0.000 wall:      0.003)
 CheckConvergence...................:      0.016 (sys:      0.000 wall:      0.007)
PDSystemSolverTotal.................:      0.032 (sys:      0.016 wall:      0.049)
 PDSystemSolverSolveOnce............:      0.028 (sys:      0.016 wall:      0.042)
 ComputeResiduals...................:      0.000 (sys:      0.000 wall:      0.003)
 StdAugSystemSolverMultiSolve.......:      0.012 (sys:      0.008 wall:      0.018)
 LinearSystemScaling................:      0.000 (sys:      0.000 wall:      0.000)
 LinearSystemSymbolicFactorization..:      0.000 (sys:      0.000 wall:      0.000)
 LinearSystemFactorization..........:      0.000 (sys:      0.000 wall:      0.004)
 LinearSystemBackSolve..............:      0.004 (sys:      0.000 wall:      0.004)
 LinearSystemStructureConverter.....:      0.000 (sys:      0.000 wall:      0.000)
  LinearSystemStructureConverterInit:      0.000 (sys:      0.000 wall:      0.000)
QualityFunctionSearch...............:      0.000 (sys:      0.000 wall:      0.005)
TryCorrector........................:      0.000 (sys:      0.000 wall:      0.000)
Task1...............................:      0.000 (sys:      0.000 wall:      0.001)
Task2...............................:      0.000 (sys:      0.000 wall:      0.001)
Task3...............................:      0.000 (sys:      0.000 wall:      0.000)
Task4...............................:      0.000 (sys:      0.000 wall:      0.000)
Task5...............................:      0.000 (sys:      0.000 wall:      0.001)
Function Evaluations................:      0.040 (sys:      0.004 wall:      0.042)
 Objective function.................:      0.000 (sys:      0.000 wall:      0.001)
 Objective function gradient........:      0.008 (sys:      0.000 wall:      0.001)
 Equality constraints...............:      0.028 (sys:      0.004 wall:      0.037)
 Inequality constraints.............:      0.004 (sys:      0.000 wall:      0.001)
 Equality constraint Jacobian.......:      0.000 (sys:      0.000 wall:      0.000)
 Inequality constraint Jacobian.....:      0.000 (sys:      0.000 wall:      0.001)
 Lagrangian Hessian.................:      0.000 (sys:      0.000 wall:      0.000)

Here is the second most long run

Number of Iterations....: 27

                                   (scaled)                 (unscaled)
Objective...............:   1.4030965622086845e-03    1.4030965622086845e-03
Dual infeasibility......:   2.1071334326346378e-05    2.1071334326346378e-05
Constraint violation....:   9.9292587017607440e-05    9.9292587017607440e-05
Complementarity.........:   1.0003472142487473e-11    1.0003472142487473e-11
Overall NLP error.......:   9.9292587017607440e-05    9.9292587017607440e-05

Number of objective function evaluations             = 78
Number of objective gradient evaluations             = 28
Number of equality constraint evaluations            = 78
Number of inequality constraint evaluations          = 78
Number of equality constraint Jacobian evaluations   = 28
Number of inequality constraint Jacobian evaluations = 28
Number of Lagrangian Hessian evaluations             = 0
Total CPU secs in IPOPT (w/o function evaluations)   =      0.008
Total CPU secs in NLP function evaluations           =      0.004

Timing Statistics:

OverallAlgorithm....................:      0.012 (sys:      0.016 wall:      0.027)
 PrintProblemStatistics.............:      0.000 (sys:      0.000 wall:      0.000)
 InitializeIterates.................:      0.000 (sys:      0.000 wall:      0.000)
 UpdateHessian......................:      0.004 (sys:      0.000 wall:      0.001)
 OutputIteration....................:      0.000 (sys:      0.000 wall:      0.000)
 UpdateBarrierParameter.............:      0.000 (sys:      0.004 wall:      0.008)
 ComputeSearchDirection.............:      0.000 (sys:      0.004 wall:      0.006)
 ComputeAcceptableTrialPoint........:      0.008 (sys:      0.008 wall:      0.010)
 AcceptTrialPoint...................:      0.000 (sys:      0.000 wall:      0.001)
 CheckConvergence...................:      0.000 (sys:      0.000 wall:      0.002)
PDSystemSolverTotal.................:      0.004 (sys:      0.008 wall:      0.011)
 PDSystemSolverSolveOnce............:      0.004 (sys:      0.008 wall:      0.009)
 ComputeResiduals...................:      0.000 (sys:      0.000 wall:      0.001)
 StdAugSystemSolverMultiSolve.......:      0.000 (sys:      0.004 wall:      0.004)
 LinearSystemScaling................:      0.000 (sys:      0.000 wall:      0.000)
 LinearSystemSymbolicFactorization..:      0.000 (sys:      0.000 wall:      0.000)
 LinearSystemFactorization..........:      0.000 (sys:      0.000 wall:      0.001)
 LinearSystemBackSolve..............:      0.000 (sys:      0.000 wall:      0.001)
 LinearSystemStructureConverter.....:      0.000 (sys:      0.000 wall:      0.000)
  LinearSystemStructureConverterInit:      0.000 (sys:      0.000 wall:      0.000)
QualityFunctionSearch...............:      0.000 (sys:      0.000 wall:      0.003)
TryCorrector........................:      0.000 (sys:      0.000 wall:      0.000)
Task1...............................:      0.000 (sys:      0.000 wall:      0.001)
Task2...............................:      0.000 (sys:      0.000 wall:      0.000)
Task3...............................:      0.000 (sys:      0.000 wall:      0.000)
Task4...............................:      0.000 (sys:      0.000 wall:      0.000)
Task5...............................:      0.000 (sys:      0.000 wall:      0.000)
Function Evaluations................:      0.004 (sys:      0.008 wall:      0.007)
 Objective function.................:      0.000 (sys:      0.004 wall:      0.000)
 Objective function gradient........:      0.000 (sys:      0.000 wall:      0.000)
 Equality constraints...............:      0.004 (sys:      0.004 wall:      0.006)
 Inequality constraints.............:      0.000 (sys:      0.000 wall:      0.000)
 Equality constraint Jacobian.......:      0.000 (sys:      0.000 wall:      0.000)
 Inequality constraint Jacobian.....:      0.000 (sys:      0.000 wall:      0.000)
 Lagrangian Hessian.................:      0.000 (sys:      0.000 wall:      0.000)

Here a more "common" run

Number of Iterations....: 7

                                   (scaled)                 (unscaled)
Objective...............:   1.4149110895861127e-03    1.4149110895861127e-03
Dual infeasibility......:   7.1185042965366023e-05    7.1185042965366023e-05
Constraint violation....:   6.0936711646090798e-07    6.0936711646090798e-07
Complementarity.........:   6.9885413724534827e-08    6.9885413724534827e-08
Overall NLP error.......:   7.1185042965366023e-05    7.1185042965366023e-05

Number of objective function evaluations             = 8
Number of objective gradient evaluations             = 8
Number of equality constraint evaluations            = 8
Number of inequality constraint evaluations          = 8
Number of equality constraint Jacobian evaluations   = 8
Number of inequality constraint Jacobian evaluations = 8
Number of Lagrangian Hessian evaluations             = 0
Total CPU secs in IPOPT (w/o function evaluations)   =      0.004
Total CPU secs in NLP function evaluations           =      0.000

Timing Statistics:

OverallAlgorithm....................:      0.004 (sys:      0.004 wall:      0.006)
 PrintProblemStatistics.............:      0.000 (sys:      0.000 wall:      0.000)
 InitializeIterates.................:      0.000 (sys:      0.000 wall:      0.000)
 UpdateHessian......................:      0.000 (sys:      0.000 wall:      0.000)
 OutputIteration....................:      0.000 (sys:      0.000 wall:      0.000)
 UpdateBarrierParameter.............:      0.000 (sys:      0.000 wall:      0.003)
 ComputeSearchDirection.............:      0.000 (sys:      0.000 wall:      0.001)
 ComputeAcceptableTrialPoint........:      0.000 (sys:      0.004 wall:      0.001)
 AcceptTrialPoint...................:      0.000 (sys:      0.000 wall:      0.000)
 CheckConvergence...................:      0.004 (sys:      0.000 wall:      0.000)
PDSystemSolverTotal.................:      0.000 (sys:      0.000 wall:      0.002)
 PDSystemSolverSolveOnce............:      0.000 (sys:      0.000 wall:      0.001)
 ComputeResiduals...................:      0.000 (sys:      0.000 wall:      0.000)
 StdAugSystemSolverMultiSolve.......:      0.000 (sys:      0.000 wall:      0.001)
 LinearSystemScaling................:      0.000 (sys:      0.000 wall:      0.000)
 LinearSystemSymbolicFactorization..:      0.000 (sys:      0.000 wall:      0.000)
 LinearSystemFactorization..........:      0.000 (sys:      0.000 wall:      0.000)
 LinearSystemBackSolve..............:      0.000 (sys:      0.000 wall:      0.000)
 LinearSystemStructureConverter.....:      0.000 (sys:      0.000 wall:      0.000)
  LinearSystemStructureConverterInit:      0.000 (sys:      0.000 wall:      0.000)
QualityFunctionSearch...............:      0.000 (sys:      0.000 wall:      0.002)
TryCorrector........................:      0.000 (sys:      0.000 wall:      0.000)
Task1...............................:      0.000 (sys:      0.000 wall:      0.000)
Task2...............................:      0.000 (sys:      0.000 wall:      0.000)
Task3...............................:      0.000 (sys:      0.000 wall:      0.000)
Task4...............................:      0.000 (sys:      0.000 wall:      0.000)
Task5...............................:      0.000 (sys:      0.000 wall:      0.000)
Function Evaluations................:      0.000 (sys:      0.000 wall:      0.001)
 Objective function.................:      0.000 (sys:      0.000 wall:      0.000)
 Objective function gradient........:      0.000 (sys:      0.000 wall:      0.000)
 Equality constraints...............:      0.000 (sys:      0.000 wall:      0.001)
 Inequality constraints.............:      0.000 (sys:      0.000 wall:      0.000)
 Equality constraint Jacobian.......:      0.000 (sys:      0.000 wall:      0.000)
 Inequality constraint Jacobian.....:      0.000 (sys:      0.000 wall:      0.000)
 Lagrangian Hessian.................:      0.000 (sys:      0.000 wall:      0.000)

francesco-romano commented 6 years ago

cc @pattacini @S-Dafarra @traversaro @DanielePucci

pattacini commented 6 years ago

Out of sheer curiosity, have you ever tried to resort to a different parametrization of the orientation, for example, the axis-angle or the quaternion?

In my experience, RPY might bring about headaches.

francesco-romano commented 6 years ago

I started developing the library by using quaternion parametrisation. Unfortunately we never tested on the robot this representation, and I remember some complains by the IPOPT derivative checker.

pattacini commented 6 years ago

@francesco-romano I had my past developments focused on the use of axis-angle, for which we have access to good descriptions of the derivative (ref. Sciavicco-Siciliano)

francesco-romano commented 6 years ago

That's great. We could extend this enum supporting also axis-angle

S-Dafarra commented 6 years ago

Just to provide some data:

Number of IK iterations: 2799
Timings
Sample Mean: 3.9614 ms
Sample Variance: 0.1205 ms
Max time: 556 ms
Min time: <1ms
Full data plot:
Plot between 1000 and 1999 IK iterations:

Iterations

Sample Mean: 5.6963 iterations
Sample Variance: 141.2587 iterations
Max number of iterations: 609
Min number of iterations: 0
Full data plot:
Plot between 1000 and 1999 IK iterations:

Ipopt was using ma27.

Data

data.zip

DanielePucci commented 6 years ago

@S-Dafarra thanks a lot for the plots! Just a curiosity, which were the characteristics of the machine used for the tests? Probably, it would be nice to write them here as per reference

lrapetti commented 5 years ago

Regarding this kind of performance analysis, I think it would be to worth to have cpp interface for reading the Statistics of the IK?

For my understanding, at the moment performance statistics (e.g. Number of Iterations) can be only retrieved setting high verbosity and parsing the std output.

traversaro commented 5 years ago

Regarding this kind of performance analysis, I think it would be to worth to have cpp interface for reading the Statistics of the IK?

I am not sure it would make sense to wrap all that in a iDynTree-specific API. I think it could make more sense to expose some way to return opaque pointers to the internal IPOPT structures (documenting in a super clear way that the method is super unsupported) in a way that Ipopt::SolveStatistics can be used in some profiling test. See std::thread::native_handle ( https://en.cppreference.com/w/cpp/thread/thread/native_handle ) to see how a similar pattern is implemented in the C++ standard library.

robotology / idyntree

Timing statistics of IK #385

Timings

Iterations

Data