Moveit rviz plugin generates invalid quaternion

edit

Hi together,

I'm trying to setup Moveit for a custom robot arm. Basically everything is working, but when running the demo generated by MoveIt! Setup Assistant I am getting a weird message from Rviz:

[DEBUG] [1523088247.990100347]: Quaternion [x: 1,000, y: 0,000, z: 0,000, w: 1,000] not normalized. Magnitude: 1,414
[ WARN] [1523088247.990177816]: Interactive marker 'EE:goal_tool0' contains unnormalized quaternions. This warning will only be output once but may be true for others; enable DEBUG messages for ros.rviz.quaternions to see more details.
[DEBUG] [1523088247.990237873]: Interactive marker 'EE:goal_tool0' contains unnormalized quaternions.

I can't understand where this is coming from. I use a custom IK plugin, but it only computes the joint angles, not the actual poses. IK still works correctly when moving around the interactive marker in Rviz.

The problem now is that when I add gripper joints and links to tool0 link, Rviz immediately crashes as soon as I move the goal interactive marker of the Moveit plugin.

What could be the issue here? My URDF is just a chain of links and revolute joints. The last link "link6" is connected to "tool0" by a fixed joint.

My move group in SRDF is this:

    group name="arm">
        <link name="tool0" />
        <joint name="joint5" />
        <joint name="joint4" />
        <joint name="joint3" />
        <joint name="joint2" />
        <joint name="joint1" />
        <joint name="link6-tool0" />
    </group>

Thanks for the help!