Add link offset to simulation

[claireyywang]

Closes #57

Link offset is added to the simulation after some update in tpeplugin. The following shows distinct offsetted pose of wheels and caster.

Screenshot of demo with velocity_control.sdf

[codecov]

Codecov Report

Merging #83 into ign-physics2 will decrease coverage by 0.27%.
The diff coverage is 54.76%.

@@               Coverage Diff                @@
##           ign-physics2      #83      +/-   ##
================================================
- Coverage         82.82%   82.54%   -0.28%     
================================================
  Files               106      106              
  Lines              3663     3679      +16     
================================================
+ Hits               3034     3037       +3     
- Misses              629      642      +13     

Impacted Files	Coverage Δ
tpe/plugin/src/SDFFeatures.cc	80.00% <50.00%> (-6.21%)	⬇️
tpe/plugin/src/FreeGroupFeatures.cc	68.88% <52.94%> (-10.06%)	⬇️
tpe/plugin/src/KinematicsFeatures.cc	52.63% <62.50%> (-3.90%)	⬇️
tpe/lib/src/Entity.cc	85.83% <100.00%> (ø)

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 1207fe0...03f78ee. Read the comment docs.

[claireyywang]

@iche033 Fortunately, I was able to figure out a fix for the problem. I think this is ready for a first pass of review 😃

[iche033]

nice, the links offset look correct now. There are just some minor code check errors.

[iche033]

do we need to do something to fix the linear and angular vel?

[claireyywang]

My understanding is that linear and angular velocity are tied to model instead of link, and this test case started checking the velocity of link after I corrected the implementation of framedata, which is why I removed it. Before it worked because when calling link->FrameDataRelativeToWorld(), it was getting the velocity of the link's parent model.

If we do want to add velocity for links, then that would be a different feature which allows links to move by itself.

[iche033]

oh I see, that makes sense. Just checking, if we assume the link does not move within a model, should it inherit the parent model's velocities in world frame?

In any case, I'm not sure if there's a use case for querying a link's world velocities. We can address this when the need comes.

[claireyywang]

If the link doesn't move within model, its WorldPose is then updated as model's WorldPose gets updated (since its offset will remain the same), so we probably don't have to keep track of link velocities in world frame.

However, if we want the link to move within model, then we'll have to compute the link velocities with the offset. And yes, I agree that we can add this if the need comes.

[iche033]

apart from the minor code check errors, the latest changes look good to me