Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

asked 2018-02-22 21:38:40 -0600

Cerin gravatar image

How to make IMU orientation reflect in Rviz

I've built a small R2D2 like robot composed of three parts, a cylindrical torso, a panning cylindrical neck, and tilting spherical head, all equally modeled in a URDF file. There's also an IMU positioned at the front-collar of the torso cylinder, and a camera positioned at the front of the head sphere.

With a tf/robot_state_publisher node running, I can visualize my robot in Rviz like:

image description

My IMU is publishing standard Imu messages to the /imu/data_raw topic, which Rviz is displaying and rendering as a separate axis. It's difficult to see from the cluttered display, but it's the unlabeled axis at the bottom that's rotated askew from the rest of the model. When I rotate my robot, and thus rotating the IMU, the IMU's axes correctly pans/tilts/yaws in Rviz. However, the rest of the robot remains stationary. If my robot starts moving via its wheels, and thus updates odometry information on the standard /odom topic, it begins moving in Rviz, but any pan/tilt/yaw is not reflected. How do I fix this?

If I'm publishing IMU data, and my IMU is represented by a linkage connected via a fixed joint to my base_link, then why isn't Rviz showing the IMU movement as a separate axis instead of apply it to the base_link?

Is the problem happening because I've not correctly linked up my IMU data to a linkage? Is it a mistake to try and apply IMU data to an IMU linkage, or should I instead merge my IMU's orientation data directly into my odometry messages and forget trying to explicitly model the IMU in URDF altogether?

I wanted to model the IMU in URDF to help TF take into account it's asymmetric placement (forward of the centerline).

My URDF looks like:

<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="myrobot">
    <xacro:include filename="$(find ros_myrobot_description)/urdf/materials.urdf.xacro" />
    <xacro:property name="torso_radius" value="0.075" /><!-- 150 mm diameter = 75 mm radius -->
    <xacro:property name="torso_height" value="0.16" /><!-- 160 mm height -->
    <xacro:property name="neck_height" value="0.015" />
    <xacro:property name="camera_radius" value="0.008" />
    <xacro:property name="camera_thickness" value="0.005" />
    <xacro:property name="M_PI" value="3.141592653589793" />

    <link name="base_link">
        <visual>
            <geometry>
                <cylinder length="${torso_height}" radius="${torso_radius}" />
            </geometry>
            <origin xyz="0 0 ${torso_height/2}" rpy="0 0 0" />
            <material name="red" />
        </visual>
    </link>

    <link name="imu">
        <visual>
            <geometry>
                <box size=".03 .015 .01" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="orange" />
        </visual>
    </link>

    <link name="neck">
        <visual>
            <geometry>
                <cylinder length="${neck_height}" radius="${torso_radius}" />
            </geometry>
            <origin xyz="0 0 ${neck_height/2}" rpy="0 0 0" />
            <material name="green" />
        </visual>
    </link>

    <link name="head">
        <visual>
            <geometry>
                <sphere radius="${torso_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="blue" />
        </visual>
    </link>

    <link name="camera">
        <visual>
            <geometry>
                <cylinder length="${camera_thickness}" radius="${camera_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 90 0" />
            <material name="black" />
        </visual>
    </link>

    <joint name="base_link_to_neck_joint" type="continuous">
        <parent link="base_link" />
        <child link="neck" />
        <origin xyz="0 0 ${torso_height+neck_height}" rpy="0 ${M_PI} 0" />
        <!--<origin xyz="0 0 ${torso_height}" rpy="0 ${M_PI} 0" />-->
        <axis xyz="0 0 1" />
    </joint>

    <joint name="base_link_to_imu" type="fixed">
        <parent link="base_link" />
        <child link="imu" />
        <origin xyz="0 ${torso_radius - .015} ${torso_height - .01}" rpy="0 0 0" />
    </joint>

    <joint name="neck_to_head_joint" type="revolute">
        <parent link="neck" />
        <child link="head" />
        <origin xyz="0 0 ${-(torso_radius)}" rpy="${M_PI/2} 0 ${M_PI}" />
        <axis xyz="1 0 0" />
        <limit lower="${(90-65)*M_PI/180}" upper="${(90+65)*M_PI/180}" effort="50" velocity="1"/>
    </joint>

    <joint name="head_to_camera_joint" type="fixed">
        <parent link="head" />
        <child link="camera" />
        <origin xyz="0 ${torso_radius} 0" rpy="0 0 0" />
    </joint>

</robot>

How to make IMU orientation reflect in Rviz

I've built a small R2D2 like robot composed of three parts, a cylindrical torso, a panning cylindrical neck, and tilting spherical head, all equally modeled in a URDF file. There's also an IMU positioned at the front-collar of the torso cylinder, and a camera positioned at the front of the head sphere.

With a tf/robot_state_publisher node running, I can visualize my robot in Rviz like:

image description

My IMU is publishing standard Imu messages to the /imu/data_raw topic, which Rviz is displaying and rendering as a separate axis. It's difficult to see from the cluttered display, but it's the unlabeled axis at the bottom that's rotated askew from the rest of the model. When I rotate my robot, and thus rotating the IMU, the IMU's axes correctly pans/tilts/yaws in Rviz. However, the rest of the robot remains stationary. For example, when I lay my robot on its side, here's how Rviz renders it:

image description

Notice the IMU's green y-axis is now pointing straight up, but the rest of the robot remains unchanged. If my robot starts moving via its wheels, and thus updates odometry information on the standard /odom topic, it begins moving in Rviz, but any pan/tilt/yaw is not reflected. How do I fix this?

If I'm publishing IMU data, and my IMU is represented by a linkage connected via a fixed joint to my base_link, then why isn't Rviz showing the IMU movement as a separate axis instead of apply it to the base_link?

Is the problem happening because I've not correctly linked up my IMU data to a linkage? Is it a mistake to try and apply IMU data to an IMU linkage, or should I instead merge my IMU's orientation data directly into my odometry messages and forget trying to explicitly model the IMU in URDF altogether?

I wanted to model the IMU in URDF to help TF take into account it's asymmetric placement (forward of the centerline).

My URDF looks like:

<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="myrobot">
    <xacro:include filename="$(find ros_myrobot_description)/urdf/materials.urdf.xacro" />
    <xacro:property name="torso_radius" value="0.075" /><!-- 150 mm diameter = 75 mm radius -->
    <xacro:property name="torso_height" value="0.16" /><!-- 160 mm height -->
    <xacro:property name="neck_height" value="0.015" />
    <xacro:property name="camera_radius" value="0.008" />
    <xacro:property name="camera_thickness" value="0.005" />
    <xacro:property name="M_PI" value="3.141592653589793" />

    <link name="base_link">
        <visual>
            <geometry>
                <cylinder length="${torso_height}" radius="${torso_radius}" />
            </geometry>
            <origin xyz="0 0 ${torso_height/2}" rpy="0 0 0" />
            <material name="red" />
        </visual>
    </link>

    <link name="imu">
        <visual>
            <geometry>
                <box size=".03 .015 .01" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="orange" />
        </visual>
    </link>

    <link name="neck">
        <visual>
            <geometry>
                <cylinder length="${neck_height}" radius="${torso_radius}" />
            </geometry>
            <origin xyz="0 0 ${neck_height/2}" rpy="0 0 0" />
            <material name="green" />
        </visual>
    </link>

    <link name="head">
        <visual>
            <geometry>
                <sphere radius="${torso_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="blue" />
        </visual>
    </link>

    <link name="camera">
        <visual>
            <geometry>
                <cylinder length="${camera_thickness}" radius="${camera_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 90 0" />
            <material name="black" />
        </visual>
    </link>

    <joint name="base_link_to_neck_joint" type="continuous">
        <parent link="base_link" />
        <child link="neck" />
        <origin xyz="0 0 ${torso_height+neck_height}" rpy="0 ${M_PI} 0" />
        <!--<origin xyz="0 0 ${torso_height}" rpy="0 ${M_PI} 0" />-->
        <axis xyz="0 0 1" />
    </joint>

    <joint name="base_link_to_imu" type="fixed">
        <parent link="base_link" />
        <child link="imu" />
        <origin xyz="0 ${torso_radius - .015} ${torso_height - .01}" rpy="0 0 0" />
    </joint>

    <joint name="neck_to_head_joint" type="revolute">
        <parent link="neck" />
        <child link="head" />
        <origin xyz="0 0 ${-(torso_radius)}" rpy="${M_PI/2} 0 ${M_PI}" />
        <axis xyz="1 0 0" />
        <limit lower="${(90-65)*M_PI/180}" upper="${(90+65)*M_PI/180}" effort="50" velocity="1"/>
    </joint>

    <joint name="head_to_camera_joint" type="fixed">
        <parent link="head" />
        <child link="camera" />
        <origin xyz="0 ${torso_radius} 0" rpy="0 0 0" />
    </joint>

</robot>