Moveit! self_filter from octomap working for robotiq gripper but not UR5/UR10 body

Question

System: Ubuntu 16.04 / ROS Kinetic / Nuvo-5095GC-022 / Universal Robots UR10 Packages: freenect_stack, geometry2, libfreenect, perception_pcl, trac_ik, universal_robot, ur_modern_driver

Hi,

Some background: As part of a dynamic collision avoidance project I'm trying to achieve full visibility of a live scene using multiple Kinect cameras. In the current setup I have two Xbox Kinect V1s, each producing a working point cloud. The cameras are positioned with a 90deg offset and the frames have been linked using tf2, so the points match up relatively well in 3D (cameras are intrinsically calibrated). From there i have used the two point clouds to create an octomap that the robot plans to avoid.

Now that i've got the live octomap sorted i'm running into problems with the integrated self_filter function inside Moveit!, in that only the gripper is being filtered from the scene regardless of the padding/offset values used. If i view the filtered_cloud topic as a point cloud 2, it clearly illustrates the problem.

It's almost like the robot urdf is being ignored due to the addition of the end effector?

UPDATE (01/11/2018). Observations:

1. Removal of the second Octomap updater plugin does not resolve the problem
2. *** I've noticed that the Octomap is not clearing correctly. If I move the Kinect, the map is 'filled' with new blocks but old blocks are not removed, meaning that they have to be cleared manually. This is likely to be the cause but I have no idea why this might be happening, i'll look into it when I can.
3. If I copy the UR10 sensor.yaml and launch files into the UR5 package, the Octomap behaves as expected (clearing correctly, self_filter operational). I think the only difference is that the UR10 has a gripper attached with all gripper joints set as passive, the UR5 has no gripper attached.

Any ideas?

My sensors.yaml file looks like:

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /camera/depth_registered/points
    max_range: 5.0
    point_subsample: 1 # As expected. Every 1 in point_subsample read
    padding_offset: 0.1
    padding_scale: 1.0 # Scale of voxels
    filtered_cloud_topic: filtered_cloud
    shape_padding: 0.5
    shape_scale: 1.0
    self_see_default_padding: 0.1
    self_see_default_scale: 1.0

  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /camera2/depth_registered/points
    max_range: 5.0
    point_subsample: 1 # As expected. Every 1 in point_subsample read
    padding_offset: 0.1
    padding_scale: 0.1 # Scale of voxels
    filtered_cloud_topic: filtered2_cloud
    shape_padding: 0.3
    shape_scale: 1.0
    self_see_default_padding: 0.5
    self_see_default_scale: 1.0

And my urdf:

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">

<!--
  Author: Kelsey Hawkins
  Contributers: Jimmy Da Silva, Ajit Krisshna N L, Muhammad Asif Rana
-->


  <xacro:include filename="$(find ur_description)/urdf/ur.transmission.xacro" />
  <xacro:include filename="$(find ur_description)/urdf/ur.gazebo.xacro" />

<!-- Robotiq --> 
  <xacro:include filename="$(find robotiq_description)/urdf/robotiq_85_gripper.urdf.xacro" />
  <xacro:robotiq_85_gripper parent="ee_link" prefix="gripper">
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </xacro:robotiq_85_gripper>

  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
    <inertial>
      <mass value="${mass}" />
      <xacro:insert_block name="origin" />
      <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
        iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
        izz="${0.5 * mass * radius * radius}" />
    </inertial>
  </xacro:macro>

  <xacro:macro name="ur10_robot" params="prefix joint_limited
         shoulder_pan_lower_limit:=${-pi}    shoulder_pan_upper_limit:=${pi}
         shoulder_lift_lower_limit:=${-pi}    shoulder_lift_upper_limit:=${pi}
         elbow_joint_lower_limit:=${-pi}    elbow_joint_upper_limit:=${pi}
         wrist_1_lower_limit:=${-pi}    wrist_1_upper_limit:=${pi}
         wrist_2_lower_limit:=${-pi}    wrist_2_upper_limit:=${pi}
         wrist_3_lower_limit:=${-pi}    wrist_3_upper_limit:=${pi}">

    <!-- Inertia parameters -->
    <xacro:property name="base_mass" value="4.0" />  <!-- This mass might be incorrect -->
    <xacro:property name="shoulder_mass" value="7.778" />
    <xacro:property name="upper_arm_mass" value="12.93" />
    <xacro:property name="forearm_mass" value="3.87" />
    <xacro:property name="wrist_1_mass" value="1.96" />
    <xacro:property name="wrist_2_mass" value="1.96" />
    <xacro:property name="wrist_3_mass" value="0.202" />

    <!-- These parameters are borrowed from the urcontrol.conf file
        but are not verified for the correct permutation.
        The permutation was guessed by looking at the UR5 parameters.
        Serious use of these parameters needs further inspection. -->
    <xacro:property name="shoulder_cog" value="0.00008 0.00244 -0.037" />
    <xacro:property name="upper_arm_cog" value="0.00001 0.15061 0.38757" />
    <xacro:property name="forearm_cog" value="-0.00012 0.06112 0.1984" />
    <xacro:property name="wrist_1_cog" value="-0.00021 -0.00112 0.02269" />
    <xacro:property name="wrist_2_cog" value="-0.00021 0.00112 0.002269" />
    <xacro:property name="wrist_3_cog" value="0 -0.001156 -0.00149" />

    <!-- Kinematic model -->
    <!-- Properties from urcontrol.conf -->
    <xacro:property name="d1" value="0.1273" />
    <xacro:property name="a2" value="-0.612" />
    <xacro:property name="a3" value="-0.5723" />
    <xacro:property name="d4" value="0.163941" />
    <xacro:property name="d5" value="0.1157" />
    <xacro:property name="d6" value="0.0922" />

    <!-- Arbitrary offsets for shoulder/elbow joints -->
    <xacro:property name="shoulder_offset" value="0.220941" />  <!-- measured from model -->
    <xacro:property name="elbow_offset" value="-0.1719" /> <!-- measured from model -->

    <!-- link lengths used in model -->
    <xacro:property name="shoulder_height" value="${d1}" />
    <xacro:property name="upper_arm_length" value="${-a2}" />
    <xacro:property name="forearm_length" value="${-a3}" />
    <xacro:property name="wrist_1_length" value="${d4 - elbow_offset - shoulder_offset}" />
    <xacro:property name="wrist_2_length" value="${d5}" />
    <xacro:property name="wrist_3_length" value="${d6}" />

    <link name="${prefix}base_link" >
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/visual/base.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/collision/base.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="0.038" mass="${base_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}shoulder_pan_joint" type="revolute">
      <parent link="${prefix}base_link" />
      <child link = "${prefix}shoulder_link" />
      <origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" />
      <axis xyz="0 0 1" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" effort="330.0" velocity="2.16"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}shoulder_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/visual/shoulder.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/collision/shoulder.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="0.178" mass="${shoulder_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}shoulder_lift_joint" type="revolute">
      <parent link="${prefix}shoulder_link" />
      <child link = "${prefix}upper_arm_link" />
      <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
      <axis xyz="0 1 0" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" effort="330.0" velocity="2.16"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}upper_arm_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/visual/upperarm.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/collision/upperarm.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="${-a2}" mass="${upper_arm_mass}">
        <origin xyz="0.0 0.0 ${-a2/2.0}" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}elbow_joint" type="revolute">
      <parent link="${prefix}upper_arm_link" />
      <child link = "${prefix}forearm_link" />
      <origin xyz="0.0 ${elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" />
      <axis xyz="0 1 0" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.15"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" effort="150.0" velocity="3.15"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}forearm_link">
      <visual>
         <geometry>
          <mesh filename="package://ur_description/meshes/ur10/visual/forearm.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/collision/forearm.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="${-a3}" mass="${forearm_mass}">
        <origin xyz="0.0 0.0 ${-a3/2.0}" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}wrist_1_joint" type="revolute">
      <parent link="${prefix}forearm_link" />
      <child link = "${prefix}wrist_1_link" />
      <origin xyz="0.0 0.0 ${forearm_length}" rpy="0.0 ${pi / 2.0} 0.0" />
      <axis xyz="0 1 0" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" effort="54.0" velocity="3.2"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}wrist_1_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/visual/wrist1.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/collision/wrist1.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_1_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}wrist_2_joint" type="revolute">
      <parent link="${prefix}wrist_1_link" />
      <child link = "${prefix}wrist_2_link" />
      <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0.0 0.0" />
      <axis xyz="0 0 1" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" effort="54.0" velocity="3.2"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}wrist_2_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/visual/wrist2.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/collision/wrist2.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_2_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}wrist_3_joint" type="revolute">
      <parent link="${prefix}wrist_2_link" />
      <child link = "${prefix}wrist_3_link" />
      <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
      <axis xyz="0 1 0" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="54.0" velocity="3.2"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}wrist_3_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/visual/wrist3.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur10/collision/wrist3.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_3_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}ee_fixed_joint" type="fixed">
      <parent link="${prefix}wrist_3_link" />
      <child link = "${prefix}ee_link" />
      <origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 ${pi/2.0}" />
    </joint>

    <link name="${prefix}ee_link">
      <collision>
        <geometry>
          <box size="0.01 0.01 0.01"/>
        </geometry>
        <origin rpy="0 0 0" xyz="-0.01 0 0"/>
      </collision>
    </link>

    <xacro:ur_arm_transmission prefix="${prefix}" />
    <xacro:ur_arm_gazebo prefix="${prefix}" />

    <!-- ROS base_link to UR 'Base' Coordinates transform -->
    <link name="${prefix}base"/>
    <joint name="${prefix}base_link-base_fixed_joint" type="fixed">
      <!-- NOTE: this rotation is only needed as long as base_link itself is
                 not corrected wrt the real robot (ie: rotated over 180
                 degrees)
      -->
      <origin xyz="0 0 0" rpy="0 0 ${-pi}"/>
      <parent link="${prefix}base_link"/>
      <child link="${prefix}base"/>
    </joint>

    <!-- Frame coincident with all-zeros TCP on UR controller -->
    <link name="${prefix}tool0"/>
    <joint name="${prefix}wrist_3_link-tool0_fixed_joint" type="fixed">
      <origin xyz="0 ${wrist_3_length} 0" rpy="${pi/-2.0} 0 0"/>
      <parent link="${prefix}wrist_3_link"/>
      <child link="${prefix}tool0"/>
    </joint>

  </xacro:macro>
</robot>

---

Originally posted by nisur on ROS Answers with karma: 28 on 2018-10-30

Post score: 0

---

Original comments

Comment by gvdhoorn on 2018-10-30:
Please attach your images directly to the question. I've given you enough karma to do that.

Also: please consider accepting answers for your previous questions (if you feel they have been answered of course).

Comment by gvdhoorn on 2018-10-31:
I'm not sure, but if you remove one of the octomap updater plugins, does it then work?

Comment by nisur on 2018-10-31:
It doesn't, no. I've noticed that I'm getting the error 'Dropping first 1 trajectory point(s) out of ##, as they occur before the current time. First valid point will be reached in...' in the gazebo launch terminal, not sure if this will contribute? For some reason it works with ur5 and no gripper!

Answer 1

Solved this a little while ago but have had a break from the project since. As much as it pains me to say it, the issue was caused by a rogue 'ur5' reference in one of the UR10 launch files (most probably sensor_manager.launch.xml). Something like:

<launch>

  <!-- This file makes it easy to include the settings for sensor managers -->  

  <!-- Params for the octomap monitor -->
  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
  <param name="octomap_resolution" type="double" value="0.06" />
  <param name="max_range" type="double" value="5.0" />

  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
  <arg name="moveit_sensor_manager" default="ur5" />
  <include file="$(find ur10_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
  
</launch>

Kicking myself, thanks for the help all the same!

---

Originally posted by nisur with karma: 28 on 2019-02-26

This answer was ACCEPTED on the original site

Post score: 0