Fixed issue where we were trying to reduce the error in two places

[marrts]

Issue causing failed tests in tesseract_planning, pointed out in tesseract-robotics/tesseract_planning#389.

[marrts]

In fixing this error pointed out in the other PR, I also noticed in the DynamicCartPoseJacCalculator::operator() method we weren't using the correct calcTransformError method, so I changed that and restructured the DynamicCartPoseJacCalculator::operator() method to use a lambda function in the same way the CartPoseJacCalculator::operator() method does for consistency sake.

[Levi-Armstrong]

Approved pending pipeline passes

[marrts]

Approved pending pipeline passes

Looks like the PlanningTest.arm_around_table_multi_threaded test failed in Ubuntu:jammy on my second commit and not my first. It passed locally on this latest version, and I don't think my second commit should have any impact on that particular test. Not sure why it failed. Rerunning.

[marrts]

Approved pending pipeline passes

Looks like this fixed one of the tesseract_examples tests, but not the other

[marrts]

The test that is still failing locally for me in the tesseract_planning repo is tesseract_examples_puzzle_piece_example_unit. I can actually get this test to pass by changing from

    if (is_target_active_)
      err = tesseract_common::calcTransformErrorJac(source_tf, target_tf);
    else
      err = tesseract_common::calcTransformErrorJac(target_tf, source_tf);

to

    if (is_target_active_)
      err = tesseract_common::calcTransformError(source_tf, target_tf);
    else
      err = tesseract_common::calcTransformError(target_tf, source_tf);

In the MatrixXd CartPoseJacCalculator::operator()(const VectorXd& dof_vals) const method.

Without that change, basically every single waypoint fails:

| ======================================================================================== |
| ---------- |  1.491e+02 |  1.490e+02 |  1.477e+02 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  1.491e+07 |  1.490e+07 | ---------- |  1.483e+05 |  1.557e+04 |  1.050e-01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] shrunk trust region. new box size: 0.0002
[INFO] converged because trust region is tiny
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_0
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_1
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_2
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_3
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_4
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_5

...
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_430
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_431
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_432
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_433
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_434
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_435
[INFO] Not all constraints are satisfied. Increasing constraint penalties for cartesian_waypoint_436
[INFO] New merit_error_coeffs: (1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 10, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 10, 10, 10, 10, 10, 10, 10, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 1e+06, 1e+06, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 10, 10, 10, 10, 10, 10, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 10, 10, 10, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 10, 10, 10, 10, 10, 10, 10, 10, 10, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 10, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06)
[INFO] optimization couldn't satisfy all constraints
[INFO] 
==================
Optimization results:
status: OPT_PENALTY_ITERATION_LIMIT
cost values: (0, 0, 0, 0, 0, 0, 0, 0.0137209, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0514127, 0, 0, 0, 0, 0.0538555, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.59933, 0, 0.325796, 0.0578333, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 81.5688, 34.4992, 111.32)
constraint violations: (0.000105056, 0.114685, 0.605606, 0.589249, 0.563741, 0.254673, 0.296932, 6.51521, 0.0608291, 7.78375, 28.4563, 2.23997, 1.15513, 1.11613, 0.00045179, 0.401619, 1.24108, 0.622173, 0.266094, 0.392221, 0.426499, 0.495613, 0.826289, 0.0364332, 0.000127256, 0.000246396, 0.000259766, 0.577888, 0.623244, 0.435959, 0.378127, 3.38058, 7.35956, 10.2189, 7.30616, 13.6793, 16.6045, 12.1362, 9.78955, 8.79352, 0.734317, 1.09171, 1.47811, 0.00897163, 6.53986e-06, 0.000411651, 0.000395662, 0.000348996, 0.000296649, 0.000277994, 0.00027397, 0.000277501, 0.000282886, 0.000298198, 0.000346851, 0.000384951, 0.000398921, 0.000366033, 0.00033303, 0.000303699, 0.000281877, 0.000268944, 0.000267602, 0.000282126, 0.00029958, 0.000277212, 0.000253023, 0.000238013, 0.000220174, 0.000174875, 0.000146836, 0.000144972, 0.000152068, 0.000165624, 0.000184219, 0.000202264, 0.000215131, 0.000223746, 0.00023068, 0.000240765, 0.000255181, 0.000264171, 0.000267169, 0.000268309, 0.000272901, 0.000277165, 0.000273558, 0.000258177, 0.000228847, 0.000189969, 0.000155797, 0.000130966, 0.000112342, 9.71172e-05, 8.52247e-05, 7.77401e-05, 7.32938e-05, 7.02333e-05, 7.23657e-05, 9.85891e-05, 0.000137799, 0.000213099, 0.000238922, 0.000244573, 0.000242765, 0.000230076, 0.000223825, 0.000218482, 0.0002133, 0.000207674, 0.000201634, 0.000195386, 0.000188876, 0.000181814, 0.000174132, 0.000165709, 0.000155825, 0.000145057, 0.000140461, 0.000139458, 0.000140422, 0.00014183, 0.000143368, 0.00012933, 9.79662e-05, 7.06782e-05, 5.37923e-05, 4.24843e-05, 3.42976e-05, 2.80655e-05, 2.29754e-05, 2.38839e-05, 4.49023e-05, 7.65287e-05, 9.88222e-05, 0.000111577, 0.000102269, 8.78956e-05, 9.2329e-05, 8.64608e-05, 7.26249e-05, 5.94726e-05, 4.7521e-05, 3.62316e-05, 2.73814e-05, 2.0959e-05, 2.33745e-05, 3.30495e-05, 4.40181e-05, 6.08399e-05, 7.72042e-05, 9.02503e-05, 0.000103289, 0.000116463, 0.000128857, 0.00013986, 0.000149326, 0.000157898, 0.000166213, 0.000174266, 0.00018205, 0.000189518, 0.000198065, 0.000208049, 0.000219216, 0.000231203, 0.000233301, 0.000190066, 0.000127842, 9.77466e-05, 9.17958e-05, 9.07256e-05, 9.00271e-05, 9.07325e-05, 9.71075e-05, 0.000116307, 0.000166389, 0.000248519, 0.000281427, 0.000283726, 0.000278956, 0.000261975, 0.000264127, 0.000276732, 0.000289704, 0.000291303, 0.000292146, 0.000294298, 0.000297561, 0.00029666, 0.000285441, 0.000268683, 0.000246327, 0.000218248, 0.000187991, 0.000163345, 0.000146471, 0.000136655, 0.00013377, 0.000138064, 0.00014967, 0.000168982, 0.000195348, 0.000227922, 0.000267718, 0.000323202, 0.000371576, 0.000365195, 0.000329927, 0.000228971, 0.000270549, 0.000280359, 0.000284976, 0.00028276, 0.000281146, 0.000276868, 0.000264183, 0.000253069, 0.000251198, 0.000251417, 0.000251932, 0.000251669, 0.000250212, 0.00024786, 0.000244042, 0.000252657, 0.000252484, 0.000259485, 0.000263713, 0.00025789, 0.00027596, 0.000281885, 0.000327385, 0.000371213, 0.000409955, 0.000427847, 0.000429865, 0.000422977, 0.000418085, 0.000390281, 0.000271439, 0.000226049, 0.000140011, 0.000100518, 0.000113468, 7.52508e-05, 3.36054e-05, 5.29875e-05, 0.000137307, 0.000239361, 0.000315186, 0.000312845, 0.000302857, 0.000302509, 0.000304727, 0.000273932, 0.000200765, 0.000122089, 6.15403e-05, 3.51138e-05, 3.39487e-05, 3.6585e-05, 4.09891e-05, 4.64618e-05, 5.30545e-05, 6.43403e-05, 8.19334e-05, 0.000119227, 0.000168167, 0.000162084, 0.000151095, 0.000199362, 0.00034023, 0.00044276, 0.000485706, 0.000499525, 0.000486243, 0.00046768, 0.000455572, 0.000459444, 0.000471678, 0.000472411, 0.000464703, 0.000442686, 0.000392684, 0.000305588, 0.000416998, 0.000410903, 0.000327992, 0.000250373, 0.000206171, 0.000183944, 0.000179419, 0.000184878, 0.0002075, 0.00024592, 0.000273401, 0.000285109, 0.000288559, 0.000287602, 0.00028898, 0.000293101, 0.00029915, 0.000304962, 0.000308198, 0.00030724, 0.000301603, 0.000290251, 0.000272589, 0.000287519, 0.000276078, 0.000273092, 0.000257366, 0.000224171, 0.000192522, 0.000194647, 0.000165896, 0.000145576, 0.000176773, 0.000159184, 0.000140375, 0.000124325, 0.000110436, 9.91402e-05, 8.91098e-05, 8.04976e-05, 7.29776e-05, 6.53562e-05, 5.72327e-05, 4.88304e-05, 4.07886e-05, 4.0285e-05, 4.15331e-05, 4.21961e-05, 4.1809e-05, 7.47595e-05, 0.000123555, 0.000190117, 0.000273882, 0.000364121, 0.000431561, 0.000449682, 0.000445281, 0.000436428, 0.000394126, 0.000338542, 0.000309462, 0.000317493, 0.000391685, 0.000390263, 0.000296727, 0.000187903, 0.000233936, 0.000256569, 0.000303236, 0.000380992, 0.000372415, 0.000323673, 0.000262109, 0.00013812, 4.98027e-05, 7.55128e-06, 6.24221e-06, 7.65131e-06, 1.16666e-05, 2.32064e-05, 3.83617e-05, 5.6586e-05, 7.60767e-05, 9.34547e-05, 0.000108225, 0.000122361, 0.000137667, 0.000155213, 0.000173361, 0.00019014, 0.00020451, 0.00021584, 0.000223739, 0.000228741, 0.00023209, 0.000233126, 0.000208436, 0.000147623, 5.92053e-05, 0.00014778, 0.000264939, 0.000349871, 0.000341452, 0.000325671, 0.000303693, 0.000280702, 0.00025531, 0.000217864, 0.000163241, 0.000125444, 0.000134956, 0.0001392, 0.000132582, 0.000118393, 0.000106443, 0.000103131, 0.000106092, 0.000115433, 0.00013441, 0.000165564, 0.000211019, 0.000269859, 0.000330636, 0.000382452, 0.000422026, 0.000444685, 0.000459308, 0.000465971, 0.000455122, 0.000446624, 0.000449499, 0.00044602, 0.000437255, 0.00042763, 0.000415526, 0.000372254, 0.000388913, 0.000447633, 0.000385747, 0.00043586, 0.000468218, 0.000480465, 0.000492559, 0.000500543, 0.000489445, 0.000486221, 0.000483672, 0.000482147, 0.000480256, 0.000477825, 0.000474642)
n func evals: 24
n qp solves: 23
==================
Info:    TrajOptMotionPlannerTask motion planning failed (Failed to find valid solution) for process input: Tesseract Composite Instruction
Debug:   Error
Info:    Planning took 118.372724 seconds.
Info:    Final trajectory is collision free
/home/tmarr/humble_ws/tesseract_devel/src/tesseract_planning/tesseract_examples/test/puzzle_piece_example_unit.cpp:25: Failure
Value of: example.run()
  Actual: false
Expected: true
[  FAILED  ] TesseractExamples.PuzzlePieceCppExampleUnit (118589 ms)
[----------] 1 test from TesseractExamples (118589 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (118589 ms total)
[  PASSED  ] 0 tests.
[  FAILED  ] 1 test, listed below:
[  FAILED  ] TesseractExamples.PuzzlePieceCppExampleUnit

[rjoomen]

Seems to be working now, right? Only thing left are some clang-tidy warnings, as for Jammy the clang-tidy is newer and catches some more issues. (Or you commit as is and I'll make a separate PR fixing these clang-tidy issues, as I'm on Jammy.)

[marrts]

Seems to be working now, right?

Everything in this repo is working fine, but this change was motivated because of a discovered 2 failing tests in tesseract_planning and based on my testing locally one of those tests will still fail. My above comment explains the testing failure and how with a change to this repo I can get the test to pass.

@Levi-Armstrong, do you have a recommendation on how we should best proceed?

[Levi-Armstrong]

I am looking into it and I think I know what the issue is. I will let you know what I find.