[TPE] Update link pose #179
Conversation
Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
tpe/lib/src/Link.cc
Outdated
| @@ -42,3 +42,44 @@ Entity &Link::AddCollision() | |||
| this->ChildrenChanged(); | |||
| return *it->second.get(); | |||
| } | |||
|
|
|||
| ////////////////////////////////////////////////// | |||
| void Link::SetLinearVelocity(const math::Vector3d _velocity) | |||
There was a problem hiding this comment.
let's pass by reference. I noticed that it's the same in Model.hh but having const & is more efficient.
Same for SetAngularVelocity
There was a problem hiding this comment.
Weirdly, I've read that it isn't always the case that a const reference is more efficient for small POD statically sized data structures because of cache behaviors. Copying the three double values in the Vector3d into the stack frame may have less overhead than repeatedly dereferencing the const Vector3d& which is allocated elsewhere, perhaps in the heap.
That's not say we shouldn't use const-reference here. In the absence of profiling, it's fair to assume that const T& will be more efficient than const T for non-primitive types. I just thought it's interesting that the common wisdom doesn't always hold.
Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
|
@iche033 I think this one is ready for another look as well :D |
|
We tried the PR with TPE and thought I'd check with you how you think we should model transformation when acting on links and what frame or reference commands should use. link_vel_cmds_2.mp4When applying the angular velocity on one of the links the model coordinates are also rotated, hence the two links start moving in the opposite direction (since they both rotate by 90 degrees in opposite directions). |
Codecov Report
@@ Coverage Diff @@
## ign-physics2 #179 +/- ##
================================================
- Coverage 83.14% 82.94% -0.20%
================================================
Files 106 106
Lines 3998 4034 +36
================================================
+ Hits 3324 3346 +22
- Misses 674 688 +14
Continue to review full report at Codecov.
|
Signed-off-by: Ian Chen <ichen@osrfoundation.org>
|
fixed velocity cmd frame of reference in ba45184 |
Gazebo has a concept of a canonical link which is always fixed to the model. It's usually the first link in the model if not specified through SDF. So I think what's happening is that in your example, one of the links is the canonical link and setting vel of that link may have caused the pose changes to affect the whole model. I think the workaround is to create a base link that is always fixed (e.g. door frame?), and use the velocity control system on two other door links. As for the reference frame, I've made some fixes in ba45184, the velocity should now be in link frame (it was in world frame before). @luca-della-vedova Can you give gazebosim/gz-sim#427 and this PR a try and see if it'll produce the correct behavior now? |
Signed-off-by: Ian Chen <ichen@osrfoundation.org>
Signed-off-by: Ian Chen <ichen@osrfoundation.org>
Signed-off-by: Ian Chen <ichen@osrfoundation.org>
Signed-off-by: Ian Chen <ichen@osrfoundation.org>
Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> Signed-off-by: Ian Chen <ichen@osrfoundation.org> Co-authored-by: Ian Chen <ichen@osrfoundation.org> Signed-off-by: Tomas Lorente <jtlorente@ekumenlabs.com>
Support updating individual link pose, related to gazebosim/gz-sim#427