Robot state never update its fixed joint transforms, potentially causing an infinite loop #3540
Description
Description
The RobotState joint transforms for the fixed joints (in variable_joint_transforms_) are never written to. While it makes no sens to use those values, they are however displayed when using RobotState::printStateInfo (or operator<<).
This causes two issues:
- Odd
[dirty] [NON-ISOMETRY]matrices are printed - If MoveIt was compiled against Eigen 3.3.7, which is the case for Ubuntu 20.04, this triggers a bug in checkIsometry causing an infinite loop when printing the robot state because of this bug.
Your environment
- ROS Distro: Noetic
- OS Version: e.g. Ubuntu 20.04
- Binary build 1.1.13-2focal.20230801.152056
Steps to reproduce
The following is an example to reproduce the issue. I had some case of infinite loops in production when trying to log the RobotState.
#include <cassert>
#include <cstdlib>
#include <iostream>
#include <memory>
#include <urdf/model.h>
#include <srdfdom/model.h>
#include <moveit/robot_state/robot_state.h>
char const * const URDF = R"(
<robot name="minibot">
<link name="root"/>
<link name="link1"/>
<link name="link2"/>
<joint name="link1_joint" type="prismatic">
<parent link="root"/>
<child link="link1"/>
<limit effort="1" velocity="1" lower="0" upper="1"/>
</joint>
<joint name="link2_joint" type="fixed">
<parent link="link1"/>
<child link="link2"/>
</joint>
</robot>
)";
char const * const SRDF = R"(
<robot name="minibot">
<virtual_joint name="world_to_root" type="fixed" parent_frame="world" child_link="root"/>
</robot>
)";
int main(int argc, char **argv)
{
auto urdf = std::make_shared<urdf::Model>();
assert(urdf->initString(URDF));
auto srdf = std::make_shared<srdf::Model>();
assert(srdf->initString(*urdf, SRDF));
moveit::core::RobotModelConstPtr model = std::make_shared<moveit::core::RobotModel>(urdf, srdf);
// A matrix causing the JacobianSVD infinite loop
Eigen::Matrix4d buggedMatrix;
buggedMatrix << NAN, NAN, 0, 0,
0, 1, 6.44, 0,
2.3e-17, -5e+303, 0.73, 0,
0, 0, 0, 1;
// Pollute the heap the robot state will use.
// This is only to make the issue more visible.
{
std::size_t const nBytes = 0x320;
std::size_t const nIsometries = nBytes / sizeof(Eigen::Isometry3d);
// Copied from RobotState::allocMemory
constexpr unsigned int extra_alignment_bytes = EIGEN_MAX_ALIGN_BYTES - 1;
// This malloc should return the same pointer as the one in RobotState::allocMemory.
void *data = malloc(nBytes + extra_alignment_bytes);
Eigen::Isometry3d *data_aligned = reinterpret_cast<Eigen::Isometry3d*>(((uintptr_t)data + extra_alignment_bytes) &
~(uintptr_t)extra_alignment_bytes);
for (std::size_t i = 0; i < nIsometries; ++i)
{
data_aligned[i].matrix().fill(42);
// Uncomment to trigger the infinite loop.
//data_aligned[i].matrix() = buggedMatrix;
}
free(data);
}
moveit::core::RobotState state {model};
state.update();
std::cout << state << std::endl;
}Expected behaviour
I expect the full state of the robot to be printed, and under the section "Joint transforms", either valid isometry matrices (e.g. identity) or nothing for the fixed joints.
Actual behaviour
The fixed joints transform are still marked as "dirty" and have the values present in memory before the creation of the robot state.
On my computer, if I run the previous snippet, I get:
Robot State @0x7ffdd79e05f0
* Position: 0
* Velocity: 0
* Acceleration: 0
* Dirty Link Transforms: NULL
* Dirty Collision Body Transforms: NULL
Joint transforms:
world_to_root: T.xyz = [0, 0, 0], Q.xyzw = [0, 0, 0, 1]
link1_joint: T.xyz = [0, 0, 0], Q.xyzw = [0, 0, 0, 1]
link2_joint [dirty]: [NON-ISOMETRY] [42, 42, 42, 42; 42, 42, 42, 42; 42, 42, 42, 42; 0, 0, 0, 1]
Link poses:
root: T.xyz = [0, 0, 0], Q.xyzw = [0, 0, 0, 1]
link1: T.xyz = [0, 0, 0], Q.xyzw = [0, 0, 0, 1]
link2: T.xyz = [0, 0, 0], Q.xyzw = [0, 0, 0, 1]
Note the line link2_joint [dirty]: [NON-ISOMETRY] [42, 42, 42, 42; 42, 42, 42, 42; 42, 42, 42, 42; 0, 0, 0, 1].
If I uncomment the data_aligned[i].matrix() = buggedMatrix; line, I get an infinite loop.