0
$\begingroup$

Rosanswers logo

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>

Originally posted by Cerin on ROS Answers with karma: 940 on 2018-02-22

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

It sounds like what you are missing is robot_localization http://docs.ros.org/lunar/api/robot_localization/html/index.html (or http://wiki.ros.org/robot_pose_ekf or hector_localization or something else similar) that will take imu data and odometry and then generate a transform that will improve upon a pure odometry based map to base_link.

It is important to have your imu frame in your urdf, the Imu message frame has to match up with the name of the imu frame in the urdf, it will see the imu to base link transform and do the right thing with it.

(I haven't it used it before so corrections to the above are welcome)


Originally posted by lucasw with karma: 8729 on 2018-02-22

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.