Optimize link checkCollision not to synchronize unnecessary links

[YutaKojio]

This PR is to speed up a collision check for links especially when the robot has many links.

Currently, Synchronize is called againt all links whenever _nUpdateStampId in the body is updated, which is incremented every time any link state changes.
However, when checking collision for a certain link, there is no need to synchronize other links.

This PR introduces FCLSpace::SynchronizeLink to synchronize only the link of interest.

The cases that this PR improves:

manip.CheckEndEffectorCollision(tEE, report)

and

for link in links:
    link.SetTransform(tLink)
    env.CheckCollision(link, body, report)

for example.

[rdiankov]

@cielavenir please check for regressions here, this is touching on some of the most critical and low level code we have. thanks

[cielavenir]

autotester: https://tiny.mujin.co.jp/t79gx

[cielavenir]

that autotester was flaky but our testing in latest_test.json is going well so far cc @ntohge

[kanbouchou]

Looks good to me. @rdiankov

[kanbouchou]

@rdiankov btw, can you please add Yuta (https://github.com/YutaKojio) to openrave member so that he can create a PR from here (this PR is from his fork).

[yoshikikanemoto]

The change looks good to me. @YutaKojio However, this PR can just be a patch version increment since no ABI change.

[YutaKojio]

@yoshikikanemoto Thank you. Updated the version and changelog to be a patch increment.

[yoshikikanemoto]

@YutaKojio can you resolve a conflict?

[YutaKojio]

@yoshikikanemoto Resolved the version conflict.

[cielavenir]

Hm I saw this but it should not be related to this MR....

2025-05-22T05:03:37.8330311Z ======================================================================
2025-05-22T05:03:37.8330449Z ERROR: test_kinematics.TestKinematics.test_bodybasic
2025-05-22T05:03:37.8330568Z ----------------------------------------------------------------------
2025-05-22T05:03:37.8330662Z Traceback (most recent call last):
2025-05-22T05:03:37.8336299Z   File "/opt/hostedtoolcache/Python/3.13.3/x64/lib/python3.13/site-packages/nose/case.py", line 171, in runTest
2025-05-22T05:03:37.8336415Z     self.test(*self.arg)
2025-05-22T05:03:37.8336499Z     ~~~~~~~~~^^^^^^^^^^^
2025-05-22T05:03:37.8336836Z   File "/home/runner/work/openrave/openrave/test/test_kinematics.py", line 141, in test_bodybasic
2025-05-22T05:03:37.8337634Z     raise ValueError('jacobian dot failed name=%s,link=%s,dofvalues=%r, deltavalues=%r, jacobiandeltatrans=%r, deltatrans=%r'%(body.GetName(), link.GetName(), dofvaluesnew, deltavalu
es, jacobiandeltatrans, deltatrans))
2025-05-22T05:03:37.8338038Z ValueError: jacobian dot failed name=pr2,link=br_caster_r_wheel_link,dofvalues=array([-3.49861572e+03,  5.52957830e+02, -4.99393642e+03, -3.43785924e+03,
2025-05-22T05:03:37.8338180Z        -9.27033100e+03, -3.64295161e+03,  7.43360641e+03,  2.28879117e+03,
2025-05-22T05:03:37.8338309Z         7.67426685e+02, -4.15514567e+03,  5.61267429e+02, -6.18513096e+03,
2025-05-22T05:03:37.8338436Z         2.71091602e-01,  2.77962201e-01,  4.24510810e-01,  8.13616950e-01,
2025-05-22T05:03:37.8338561Z         4.86090710e-01, -4.38467669e-01, -4.19703390e-01, -6.75458451e+03,
2025-05-22T05:03:37.8338813Z        -6.26926638e-01,  8.46350917e+03,  1.86948062e-01, -9.41220800e-02,
2025-05-22T05:03:37.8338940Z         3.87583096e+03,  8.71816113e-03,  1.04340393e+00, -1.49721290e+00,
2025-05-22T05:03:37.8339319Z         9.26382827e-01, -3.24110567e-01, -1.86464455e+00,  4.35644047e+03,
2025-05-22T05:03:37.8339454Z        -1.48987465e+00, -4.68485710e+03,  1.90516489e-01,  1.20530207e-02,
2025-05-22T05:03:37.8339767Z         7.84615267e+03,  3.07898921e-02, -3.96050375e+03]), deltavalues=array([ 1.00000000e-02,  1.00000000e-02,  1.00000000e-02, -1.00000000e-02,
2025-05-22T05:03:37.8339896Z         1.00000000e-02, -1.00000000e-02, -1.00000000e-02,  0.00000000e+00,
2025-05-22T05:03:37.8340085Z        -1.00000000e-02, -1.00000000e-02,  0.00000000e+00,  1.00000000e-02,
2025-05-22T05:03:37.8340216Z        -1.00000000e-02, -1.00000000e-02,  1.00000000e-02, -1.00000000e-02,
2025-05-22T05:03:37.8340336Z        -1.00000000e-02,  1.11022302e-16,  1.00000000e-02,  1.00000000e-02,
2025-05-22T05:03:37.8340458Z         0.00000000e+00,  1.00000000e-02,  1.00000000e-02, -1.11022302e-16,
2025-05-22T05:03:37.8340584Z         1.00000000e-02,  9.19403442e-17, -2.22044605e-16,  0.00000000e+00,
2025-05-22T05:03:37.8340709Z        -1.00000000e-02, -1.00000000e-02, -1.00000000e-02,  1.00000000e-02,
2025-05-22T05:03:37.8340832Z        -1.00000000e-02, -1.00000000e-02,  1.00000000e-02, -1.00000000e-02,
2025-05-22T05:03:37.8341291Z        -1.00000000e-02, -2.15105711e-16,  1.00000000e-02]), jacobiandeltatrans=array([-1.15542614e-05,  2.09887909e-05,  2.28721647e-06]), deltatrans=array([-3.39959560e-05,  1.15892
946e-05,  2.67369781e-05])
2025-05-22T05:03:37.8341309Z 
2025-05-22T05:03:37.8341392Z ----------------------------------------
2025-05-22T05:03:37.8341469Z Ran 152 tests in 1307.737s
2025-05-22T05:03:37.8341475Z 
2025-05-22T05:03:37.8341559Z FAILED (SKIP=20, errors=1)

[YutaKojio]

I think it's not related to this PR, but I'm not sure the cause.
Looks like #1499 has a similar test failure.

[cielavenir]

for now leave the CI matter to #1515

[cielavenir]

@kanbouchou could you check again just in case?

[cielavenir]

This branch is repeatedly tested in latest_test pipeline, so it should be ok.

[rdiankov]

Thanks!