Benchmark robot state #2546
Benchmark robot state #2546
Conversation
Codecov ReportAttention:
Additional details and impacted files@@ Coverage Diff @@
## main #2546 +/- ##
==========================================
- Coverage 50.92% 50.87% -0.04%
==========================================
Files 388 388
Lines 32274 32253 -21
==========================================
- Hits 16431 16407 -24
- Misses 15843 15846 +3 ☔ View full report in Codecov by Sentry. |
moveit_core/robot_state/include/moveit/robot_state/robot_state.h
Outdated
Show resolved
Hide resolved
moveit_core/robot_state/include/moveit/robot_state/robot_state.h
Outdated
Show resolved
Hide resolved
|
@marioprats, the optimized memory allocation was there for a reason. Allocating a single chunk of memory for the RobotState not only speeds up allocation but primarily ensures that a state's memory fits into the CPU cache (and is not scattered around). Both, allocation and caching are important for planning, which needs to allocate and update hundreds of states. You mention "at least a couple of memory issues" that are fixed by that change. Could you please name a few? |
|
Hi @rhaschke, thanks for following up on this. One example of an issue is the one referenced above in MTC (ros-planning/moveit_task_constructor#518). That's because of an out-of-bounds memory access in Here's another example, part of this PR: That was setting This PR also fixed an issue in The previous referenced issue in MTC made me realize there's a few other functions in |
|
The added complexity was nicely hidden in the init procedure. A user of RobotState doesn't notice. |
| // set velocity & acceleration to 0 | ||
| memset(velocity_, 0, sizeof(double) * 2 * robot_model_->getVariableCount()); | ||
| memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() * sizeof(unsigned char)); | ||
| std::fill(velocity_.begin(), velocity_.end(), 0); |
There was a problem hiding this comment.
This doesn't set acceleration to zero anymore as stated by the comment.
There was a problem hiding this comment.
You are right, thanks. I'll send a PR to fix this.
There was a problem hiding this comment.
To close the loop on this, here's a fix for this issue: #2617
There was a problem hiding this comment.
I don't see the missing initialization of the accelerations in #2617 either.
There was a problem hiding this comment.
Sorry, wrong PR. Should be this: #2618
| // The root joint doesn't have any variables: avoid calling getJointTransform() on it. | ||
| global_link_transforms_[idx_link] = Eigen::Isometry3d::Identity(); |
There was a problem hiding this comment.
Calling getJointTransform() (or actually accessing the joint position of a fixed joint) just became a problem with this PR. Before, the (invalid) address of the position entry was passed but actually not accessed!
There was a problem hiding this comment.
See for instance this public method, before this change:
I believe that joint->getFirstVariableIndex() has an out-of-bounds value if you call this for a fixed joint at the end of the tree, e.g. it will be 10 for a position_ size of 10, therefore pointing out of bounds. Sure, it would have returned (not accessed) an invalid address, but a user can mistakenly access it, since this is in the public API. Similarly, for a fixed joint somewhere in between the tree this would return a valid address, but pointing to data belonging to a different joint.
I consider that an issue and bad API design in general.
We are planning to make deeper changes in the design of core types to make them harder to misuse (we also had issues with tests failing randomly because of uninitialized data members). Having a more hardened memory model is a step towards that. A challenge is that we don't have great test coverage or benchmarks. We have also been improving on that little by little (see RobotState and RobotTrajectory benchmarks) but there's still a lot to do. Please let me know if you are aware of any benchmark that we can run higher in the planning stack to better measure the impact of this change.
There was a problem hiding this comment.
Yes, the pointer API can be misused, but I think it wasn't in the existing code base. However, this "risk" came for improved performance. I think the better performance in caching and memory allocation gets more pronounced if you are running short of memory, which nowadays is less than a problem than in the past...
In any case, we are balancing security for performance. With your PR you decided to weigh the former more.
Please also note that changing basic data structures also introduces the risk of new bugs - for example the missing initialization of accelerations in your PR.
Description
This change simplifies memory management in
RobotState, at the expense of slower copy/construct operations.Instead of having a
void *memory_chunk of memory with crafted pointers pointing inside, this change updates the pointer variables to be just std vectors.Construction is more expensive because now it has to do multiple allocations/initializations, instead of a single one. On the positive side, the code is more readable, maintainable and less error-prone. This change actually fixes at least a couple of memory issues with the existing code.
There is also a path to re-gain performance by not allocating / initializing data that is rarely used, and compute that data only when it's needed (e.g joint transforms, or collision body transforms). This change would make that easier.
On the performance check, these are the
RobotStatebenchmark results for the related test cases:So, state updates and FK are the same. Copy and construction are around 4x slower, e.g. copying 10.000 states changes from 0.6ms to ~2ms.
Regarding trajectories:
Creating a trajectory with 100.000 states goes from 190ms to 244ms.
Checklist