Simulated 3-axis camera gimbal for UAV in gazebo

Question

Hi there,

I'm currently trying to simulate a "perfect" (no friction) 3-axis camera gimbal attached on the bottom of a UAV. I'm using the hector quadrotor gazebo/ROS project to try and achieve this.

The ultimate goal is that the simulated image plane is continuously perpendicular with the world plane.

In order to achieve this, I've attached a simulated camera to 3 orthogonal, continuous joints via links with no length (see the URDF/xacro below). However, the image feed produced is still as if the camera were permanently affixed to it.

I'm fairly new to URDF/xacro/etc. so please excuse me if this is just a basic misunderstanding on my part!

Here's the gimbal URDF/xacro file I've created:

<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:property name="M_PI" value="3.1415926535897931" />

  <xacro:macro name="gimbal_camera" params="name parent *origin ros_topic cam_info_topic update_rate res_x res_y image_format hfov">

    <!-- Gimbal attached to UAV -->
    <joint name="gimbal_joint" type="fixed">
        <xacro:insert_block name="origin" />
        <parent link="${parent}" />
        <child link="gimbal_r_link" />
    </joint>

    <link name="gimbal_r_link">
      <inertial>
        <mass value="0" />
        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
      </inertial>
    </link>

    <joint name="gimbal_r_joint" type="continuous">
        <parent link="gimbal_r_link" />
        <child link="gimbal_p_link" />
        <axis xyz="1 0 0" />
        <dynamics damping="0.0" friction="0.0" />
    </joint>

    <link name="gimbal_p_link">
      <inertial>
        <mass value="0" />
        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
      </inertial>
    </link>

    <joint name="gimbal_p_joint" type="continuous">
        <parent link="gimbal_p_link" />
        <child link="gimbal_y_link" />
        <axis xyz="0 1 0" />
        <dynamics damping="0.0" friction="0.0" />
    </joint>

    <link name="gimbal_y_link">
      <inertial>
        <mass value="0" />
        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
      </inertial>
    </link>

    <joint name="gimbal_y_joint" type="continuous">
        <parent link="gimbal_y_link" />
        <child link="${name}_link" />
        <axis xyz="0 0 1" />
        <dynamics damping="0.0" friction="0.0" />
    </joint>
    
    <link name="${name}_link">
    </link>
    
    <joint name="${name}_optical_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
      <parent link="${name}_link" />
      <child link="${name}_optical_frame"/>
    </joint>
  
    <link name="${name}_optical_frame">
    </link>
    
    <gazebo reference="gimbal_r_link">
      <sensor type="camera" name="${name}_camera_sensor">
        <update_rate>${update_rate}</update_rate>
        <camera>
          <horizontal_fov>${hfov * M_PI/180.0}</horizontal_fov>
          <image>
            <format>${image_format}</format>
            <width>${res_x}</width>
            <height>${res_y}</height>
          </image>
          <clip>
            <near>0.01</near>
            <far>100</far>
          </clip>
        </camera>

        <plugin name="${name}_camera_controller" filename="libgazebo_ros_camera.so">
          <cameraName>${name}</cameraName>
          <imageTopicName>${ros_topic}</imageTopicName>
          <cameraInfoTopicName>${cam_info_topic}</cameraInfoTopicName>
          <frameName>${name}_optical_frame</frameName>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>

</robot>

Which is instantiated in the following file:

<?xml version="1.0"?>

<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
   
    <xacro:property name="M_PI" value="3.1415926535897931" />
    
    <!-- Included URDF Files -->
    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_base.urdf.xacro" />
    <xacro:include filename="$(find hector_sensors_description)/urdf/gimbal_camera.urdf.xacro" />
    
    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
    <xacro:quadrotor_base_macro />

    <!-- Downward facing camera attached to gimbal -->
    <xacro:gimbal_camera name="downward_cam" parent="base_link" ros_topic="camera/image" cam_info_topic="camera/camera_info" update_rate="20" res_x="640" res_y="480" image_format="R8G8B8" hfov="100">
        <origin xyz="0.4 0.0 -0.0" rpy="0 ${M_PI/2} 0"/>
    </xacro:gimbal_camera>
    
</robot>

Thanks in advance!

---

Originally posted by WillAndrew on ROS Answers with karma: 20 on 2017-09-01

Post score: 0

Answer 1

You could try adding small masses and inertia values to your links. Once you do that the camera might at least flop around in response to movements of the uav, though it likely won't be stabilized.

For that you would need to set up controllers for each joint that will command them to the proper rotation that will cancel out rotation of the parent uav frame with respect to the world. ( http://gazebosim.org/tutorials/?tut=ros_control has the basics of setting up controllers )

In order to come up with the proper commands to send to the controllers you would need to look up the transform tf of the parent uav frame with the world and pull out roll pitch and yaw. A simulated imu could be added to your uav for this- I'm not sure if the standard gazebo sim imu outputs a tf directly or just raw angles- in the latter case the raw angles could be used in your camera stabilizing controller directly. Or it only outputs raw angular rates and has to be hooked up to another node that integrates those into angles and deal with build-up of error, or use a tilt sensor instead. Or you can cheat and get the true gazebo attitude of the uav by querying gazebo directly.

It would be interesting to experiment with gyroscopic stabilization (put a motor and a spinning disc with a mass near the camera), but possibly the gazebo physics engine won't handle that properly.

---

Originally posted by lucasw with karma: 8729 on 2017-09-01

This answer was ACCEPTED on the original site

Post score: 1