idyntree
idyntree copied to clipboard
Published
20 hours ago •
robotology
robotology
Timing statistics of IK
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)
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.
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.
@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)
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
@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
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.
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.
