Moved objects out of collision

bkb_kitting_multi_10_rgbd.sdf

<sdf xmlns:vicarious="http://corp.vicarious.com/sdf/schema" version="1.8">
  <!-- based on shapes.sdf example -->
  <world name="bkb_kitting">
    <physics name="physics_settings" type="dart">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1.0</real_time_factor>
    </physics>
    <plugin filename="libignition-gazebo-physics-system.so" name="ignition::gazebo::systems::Physics"/>
    <plugin filename="libignition-gazebo-user-commands-system.so" name="ignition::gazebo::systems::UserCommands"/>
    <plugin filename="libignition-gazebo-scene-broadcaster-system.so" name="ignition::gazebo::systems::SceneBroadcaster"/>
    <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
      <render_engine>ogre2</render_engine>
    </plugin>
    <plugin filename="libignition-gazebo-logical-camera-system.so" name="ignition::gazebo::systems::LogicalCamera">
      <render_engine>ogre2</render_engine>
    </plugin>
    <plugin filename="libignition-gazebo-contact-system.so" name="ignition::gazebo::systems::Contact"/>
    <scene>
      <grid>true</grid>
      <ambient>1.0 1.0 1.0</ambient>
      <background>0.8 0.8 0.8</background>
    </scene>
    <gui fullscreen="0">
      <!-- 3D scene -->
      <plugin filename="GzScene3D" name="3D View">
        <ignition-gui>
          <title>3D View</title>
          <property type="bool" key="showTitleBar">false</property>
          <property type="string" key="state">docked</property>
        </ignition-gui>
        <engine>ogre2</engine>
        <scene>scene</scene>
        <ambient_light>1.0 1.0 1.0</ambient_light>
        <background_color>0.8 0.8 0.8</background_color>
        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
      </plugin>
      <!-- World control -->
      <plugin filename="WorldControl" name="World control">
        <ignition-gui>
          <title>World control</title>
          <property type="bool" key="showTitleBar">false</property>
          <property type="bool" key="resizable">false</property>
          <property type="double" key="height">72</property>
          <property type="double" key="width">121</property>
          <property type="double" key="z">1</property>
          <property type="string" key="state">floating</property>
          <anchors target="3D View">
            <line own="left" target="left"/>
            <line own="bottom" target="bottom"/>
          </anchors>
        </ignition-gui>
        <play_pause>true</play_pause>
        <step>true</step>
        <start_paused>true</start_paused>
        <service>/world/bkb_kitting/control</service>
        <stats_topic>/world/bkb_kitting/stats</stats_topic>
      </plugin>
      <!-- World statistics -->
      <plugin filename="WorldStats" name="World stats">
        <ignition-gui>
          <title>World stats</title>
          <property type="bool" key="showTitleBar">false</property>
          <property type="bool" key="resizable">false</property>
          <property type="double" key="height">110</property>
          <property type="double" key="width">290</property>
          <property type="double" key="z">1</property>
          <property type="string" key="state">floating</property>
          <anchors target="3D View">
            <line own="right" target="right"/>
            <line own="bottom" target="bottom"/>
          </anchors>
        </ignition-gui>
        <sim_time>true</sim_time>
        <real_time>true</real_time>
        <real_time_factor>true</real_time_factor>
        <iterations>true</iterations>
        <topic>/world/bkb_kitting/stats</topic>
      </plugin>
      <!-- Entity tree -->
      <plugin filename="EntityTree" name="Entity tree">
        <ignition-gui>
          <property type="bool" key="showTitleBar">false</property>
          <property type="string" key="state">docked</property>
        </ignition-gui>
      </plugin>
      <!-- Transform Control -->
      <plugin filename="TransformControl" name="Transform Control"/>
      <plugin filename="ImageDisplay" name="Image Display">
        <ignition-gui>
          <property key="state" type="string">docked</property>
        </ignition-gui>
      </plugin>
    </gui>
    <light type="directional" name="sun">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.8 0.8 0.8 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>
    <model name="ground_plane">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
            </plane>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
        </visual>
      </link>
    </model>
    <model name="main_arm">
      <pose>-0.75 0 .80 0 0 -1.57</pose>
      <joint name="fixed" type="fixed">
        <parent>world</parent>
        <child>case::base_link</child>
      </joint>
      <!-- The main robot arm -->
      <include>
        <uri>model://case</uri>
        <name>case</name>
        <plugin filename="libignition-vicarious-multi-joint-position-controller-system.so" name="vc_systems::MultiJointPositionController">
          <joints_n>6</joints_n>
          <joint_name_1>shoulder_pan_joint</joint_name_1>
          <joint_name_2>shoulder_lift_joint</joint_name_2>
          <joint_name_3>elbow_joint</joint_name_3>
          <joint_name_4>wrist_1_joint</joint_name_4>
          <joint_name_5>wrist_2_joint</joint_name_5>
          <joint_name_6>wrist_3_joint</joint_name_6>
          <initial_position_1>0</initial_position_1>
          <initial_position_2>-1.65</initial_position_2>
          <initial_position_3>1.61</initial_position_3>
          <initial_position_4>-1.52</initial_position_4>
          <initial_position_5>-1.58</initial_position_5>
          <initial_position_6>-0.28</initial_position_6>
          <topic_cmd_joint_pos>/joints/cmd_pos</topic_cmd_joint_pos>
          <topic_joint_state>/joints/state</topic_joint_state>
          <topic_cmd_joint_pos_reset>/joints/cmd_pos_reset</topic_cmd_joint_pos_reset>
        </plugin>
      </include>
      <!-- The suction gripper (for ignition)  -->
      <include>
        <name>suction_gripper</name>
        <pose relative_to="case::gripper_tip"/>
        <uri>model://suction_gripper</uri>
        <plugin filename="libignition-vicarious-suction-gripper-system.so" name="vc_systems::SuctionGripper">
          <main_link>main_link</main_link>
          <topic_gripping_indicated>suction/gripping_indicated</topic_gripping_indicated>
          <topic_suction_on>suction/suction_on</topic_suction_on>
        </plugin>
      </include>
      <!-- Join suction gripper to robot arm -->
      <joint name="attach_gripper" type="fixed">
        <parent>case::wrist_3_link</parent>
        <child>suction_gripper::main_link</child>
      </joint>
    </model>
    <model name="grasp_workspace_1">
      <static>1</static>
      <pose relative_to="BLB_1">0 0 0.5 0 0 0</pose>
      <link name="body">
        <visual name="workspace">
          <vicarious:logical_volume_workspace/>
          <meta>
            <layer>1</layer>
          </meta>
          <visibility_flags>0</visibility_flags>
          <geometry>
            <box>
              <size>0.5 0.5 0.4</size>
            </box>
          </geometry>
        </visual>
      </link>
    </model>
    <model name="BLB_1">
      <static>true</static>
      <pose>-.75 -.75 .3 0 0 0</pose>
      <link name="body">
        <inertial>
          <mass>0.5</mass>
          <inertia>
            <ixx>0.05</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.05</iyy>
            <iyz>0</iyz>
            <izz>0.05</izz>
          </inertia>
        </inertial>
        <visual name="visual">
          <geometry>
            <box>
              <size>.5 .5 .60</size>
            </box>
          </geometry>
          <material>
            <ambient>0.0 1.0 1.0 1</ambient>
            <diffuse>0.0 1.0 1.0 1</diffuse>
            <specular>0.5 0.5 0.5 1</specular>
          </material>
        </visual>
        <collision name="collision">
          <geometry>
            <box>
              <size>.5 .5 .60</size>
            </box>
          </geometry>
        </collision>
      </link>
    </model>
    <include>
      <uri>skin_nutrition</uri>
      <name>box-1</name>
      <pose>-0.593776 -0.65452 0.612499 0 0 0</pose>
    </include>
    <include>
      <uri>skin_nutrition</uri>
      <name>box-2</name>
      <pose>-0.9 -0.623591 0.6125 0 -0 0</pose>
    </include>
    <include>
      <uri>skin_nutrition</uri>
      <name>box-3</name>
      <pose>-0.825666 -0.757095 0.6125 0 -0 0</pose>
    </include>
    <include>
      <uri>skin_nutrition</uri>
      <name>box-4</name>
      <pose>-0.92427 -0.674616 0.612499 0 0 0.284429</pose>
    </include>
    <include>
      <uri>skin_nutrition</uri>
      <name>box-5</name>
      <pose>-0.711011 -0.77693 0.612499 0 -0 0.294069</pose>
    </include>
    <include>
      <uri>skin_nutrition</uri>
      <name>box-6</name>
      <pose>-0.691274 -0.717439 0.612499 0 -0 0.334535</pose>
    </include>
    <include>
      <uri>skin_nutrition</uri>
      <name>box-7</name>
      <pose>-0.777415 -0.687196 0.612499 0 0 0</pose>
    </include>
    <include>
      <uri>skin_nutrition</uri>
      <name>box-8</name>
      <pose>-0.691615 -0.626511 0.612499 0 -0 0.398391</pose>
    </include>
    <include>
      <uri>skin_nutrition</uri>
      <name>box-9</name>
      <pose>-0.783307 -0.614975 0.6125 0 -0 0.72</pose>
    </include>
    <include>
      <uri>skin_nutrition</uri>
      <name>box-10</name>
      <pose>-0.905051 -0.730613 0.612499 0 -0 0.72</pose>
    </include>
    <model name="rgbd_camera">
      <static>true</static>
      <pose>-.75 -.75 1.5 0 1.57 0</pose>
      <link name="rgbd_camera_link">
        <pose>0 0 0 0 0 0</pose>
        <collision name="collision">
          <geometry>
            <box>
              <size>0.1 0.1 0.1</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.1 0.1 0.1</size>
            </box>
          </geometry>
        </visual>
        <sensor name="realsense_grasp_center" type="rgbd_camera">
          <update_rate>10</update_rate>
          <topic>realsense_grasp_center</topic>
          <camera>
            <horizontal_fov>1.22</horizontal_fov>
            <image>
              <width>1280</width>
              <height>720</height>
            </image>
            <clip>
              <near>0.1</near>
              <far>10.0</far>
            </clip>
          </camera>
        </sensor>
      </link>
    </model>
  </world>
</sdf>