Fixes to inertia matrices, now only ground collision will crash

src/btree_robot/src/robot_test.cpp

@@ -90,7 +90,7 @@ int main(int argc, char** argv)
   // Visualization
   // ^^^^^^^^^^^^^
   namespace rvt = rviz_visual_tools;
-  moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "base_bottom", "move_group_tutorial",
+  moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "base_bottom", "iisy",
                                                       move_group.getRobotModel());
 
   visual_tools.deleteAllMarkers();

src/iisy_config/launch/debug.launch.py

@@ -38,7 +38,6 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
-        bringup,
         SetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE', value='true'),
 
         IncludeLaunchDescription(
@@ -52,5 +51,7 @@ def generate_launch_description():
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
         ),
-        spawn_entity
+        
+        spawn_entity,
+        bringup,
     ])

src/iisy_config/launch/lbr_move_group.launch.py

@@ -139,9 +139,23 @@ def launch_setup(context, *args, **kwargs):
         ]
     )
 
+    # MoveGroupInterface demo executable
+    move_group_demo = Node(
+        name="btree_robot",
+        package="btree_robot",
+        executable="robot_test",
+        output="screen",
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            kinematics_yaml,
+        ],
+    )
+
     return [
         move_group_node,
-        rviz
+        rviz,
+        move_group_demo
     ]
 
 def generate_launch_description():

src/iisy_config/srdf/iisy.srdf

@@ -27,7 +27,7 @@
         <joint name="link3_rot" value="0"/>
     </group_state>
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <disable_collisions link1="base" link2="base_top" reason="Adjacent"/>
+    <disable_collisions link1="base_bottom" link2="base_top" reason="Adjacent"/>
     <disable_collisions link1="base_top" link2="link1_full" reason="Adjacent"/>
     <disable_collisions link1="base_top" link2="link2_bottom" reason="Never"/>
     <disable_collisions link1="link1_full" link2="link2_bottom" reason="Adjacent"/>

src/iisy_config/urdf/iisy.urdf

@@ -5,7 +5,7 @@
 
     <link name="world"/>
     <link name="base_link"/>
-    <link name="base">
+    <link name="base_bottom">
         <collision>
             <geometry>
                 <mesh filename="file://$(find iisy_config)/stl/base_bottom_coll.stl"/>
@@ -18,9 +18,9 @@
         </visual>
 
         <inertial>
-            <origin xyz="0.025 0 0.05" rpy="0 0 0"/>
+            <origin xyz="-0.043517 0.000000 0.054914" rpy="0 0 0"/>
             <mass value="6"/>
-            <inertia ixx="0.0032666666666667" ixy="0.0" ixz="0.0" iyy="0.0032666666666667" iyz="0.0" izz="0.0025"/>
+            <inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/>
         </inertial>
 
     </link>
@@ -38,9 +38,9 @@
         </visual>
 
         <inertial>
-            <origin xyz="0 0 0.155" rpy="0 0 0"/>
+            <origin xyz="0 -0.005 0.08" rpy="0 0 0"/>
             <mass value="2"/>
-            <inertia ixx="0.0032666666666667" ixy="0.0" ixz="0.0" iyy="0.0032666666666667" iyz="0.0" izz="0.0025"/>
+            <inertia ixx="0.32666666666667" ixy="0.0" ixz="0.0" iyy="0.32666666666667" iyz="0.0" izz="0.25"/>
         </inertial>
 
     </link>
@@ -57,9 +57,9 @@
         </visual>
 
         <inertial>
-            <origin xyz="0 0 0.36" rpy="0 0 0"/>
+            <origin xyz="0 -0.050000 0.150000" rpy="0 0 0"/>
             <mass value="4"/>
-            <inertia ixx="0.0325" ixy="0.0" ixz="0.0" iyy="0.0325" iyz="0.0" izz="0.005"/>
+            <inertia ixx="8.266347" ixy="0.0" ixz="0.0" iyy="8.647519" iyz="0.0" izz="8.647519"/>
         </inertial>
 
     </link>
@@ -77,9 +77,9 @@
         </visual>
 
         <inertial>
-            <origin xyz="0 0 0.585" rpy="0 0 0"/>
+            <origin xyz="0 0.07 0.025" rpy="0 0 0"/>
             <mass value="2"/>
-            <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.0025"/>
+            <inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.25"/>
         </inertial>
 
     </link>
@@ -96,9 +96,9 @@
         </visual>
 
         <inertial>
-            <origin xyz="0 0 0.735" rpy="0 0 0"/>
+            <origin xyz="0 0.037363 0.086674" rpy="0 0 0"/>
             <mass value="2"/>
-            <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.0025"/>
+            <inertia ixx="1.431738" ixy="0.0" ixz="0.0" iyy="1.131426" iyz="-0.342192" izz="0.807202"/>
         </inertial>
 
     </link>
@@ -115,9 +115,9 @@
         </visual>
 
         <inertial>
-            <origin xyz="0 0 0.86" rpy="0 0 0"/>
+            <origin xyz="0 -0.055000 0.021413" rpy="0 0 0"/>
             <mass value="2"/>
-            <inertia ixx="0.0029166666666667" ixy="0.0" ixz="0.0" iyy="0.0029166666666667" iyz="0.0" izz="0.0025"/>
+            <inertia ixx="0.362124" ixy="0.0" ixz="0.0" iyy="0.343531" iyz="0.0" izz="0.283368"/>
         </inertial>
 
     </link>
@@ -134,9 +134,9 @@
         </visual>
 
         <inertial>
-            <origin xyz="0 0 0.94025" rpy="0 0 0"/>
+            <origin xyz="0 -0.015462 0.040000" rpy="0 0 0"/>
             <mass value="0.8"/>
-            <inertia ixx="0.00074401666666667" ixy="0.0" ixz="0.0" iyy="0.00074401666666667" iyz="0.0" izz="0.001"/>
+            <inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/>
         </inertial>
 
     </link>
@@ -148,11 +148,11 @@
     <joint type="fixed" name="base_base_link_fixed">
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <parent link="base_link"/>
-        <child link="base"/>
+        <child link="base_bottom"/>
     </joint>
     <joint type="continuous" name="base_rot">
         <origin xyz="0 0 0.1205" rpy="0 0 0"/>
-        <parent link="base"/>
+        <parent link="base_bottom"/>
         <child link="base_top"/>
         <axis xyz="0 0 1"/>
         <limit effort="30" velocity="1.7453292519943"/>
@@ -362,7 +362,7 @@
         </plugin>
     </gazebo>
 
-    <gazebo reference="base">
+    <gazebo reference="base_bottom">
         <mu1>0.2</mu1>
         <mu2>0.2</mu2>
         <material>Gazebo/Orange</material>

src/moveit_visual_tools

@@ -0,0 +1 @@
+Subproject commit ef16670fab41acb5a9f265ce2de6d7875c6358eb