Update files to mock nomenclature and add common inertias as macros. (S…

…toglRobotics#89)
Robotgir · Dec 6, 2022 · 8550721 · 8550721
1 parent 19573ff
commit 8550721
Show file tree

Hide file tree

Showing 6 changed files with 119 additions and 53 deletions.
diff --git a/templates/robot_description/common.xacro b/templates/robot_description/common.xacro
@@ -1,20 +1,63 @@
-<?xml version="1.0" encoding="UTF-8"?>
+<?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
+  <!-- see https://secure.wikimedia.org/wikipedia/en/wiki/List_of_moment_of_inertia_tensors -->
+  <xacro:macro name="sphere_inertial" params="radius mass *origin">
+    <inertial>
+      <mass value="${mass}" />
+      <xacro:insert_block name="origin" />
+      <inertia ixx="${2.0/5 * mass * radius * radius}" ixy="0.0" ixz="0.0"
+        iyy="${2.0/5 * mass * radius * radius}" iyz="0.0"
+        izz="${2.0/5 * mass * radius * radius}" />
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
+    <inertial>
+      <mass value="${mass}" />
+      <xacro:insert_block name="origin" />
+      <inertia ixx="${1.0/12 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
+        iyy="${1.0/12 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
+        izz="${1.0/2 * mass * radius * radius}" />
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="box_inertial" params="x y z mass *origin">
+    <inertial>
+      <mass value="${mass}" />
+      <xacro:insert_block name="origin" />
+      <inertia ixx="${1.0/12 * mass * (y*y + z*z)}" ixy="0.0" ixz="0.0"
+        iyy="${1.0/12 * mass * (x*x + z*z)}" iyz="0.0"
+        izz="${1.0/12 * mass * (x*x + y*y)}" />
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="spherical_cap_inertial" params="r h mass *origin">
+    <inertial>
+      <mass value="${mass}" />
+      <xacro:insert_block name="origin" />
+      <inertia ixx="${(-3*h*h*h/20+3*h*h*r/4-4*h*r*r/3+r*r*r)*3*mass/(3*r-h)+(3/4*(h-2*r)*(h-2*r)/(3*r-h))*(3/4*(h-2*r)*(h-2*r)/(3*r-h))}" ixy="0.0" ixz="0.0"
+        iyy="${(-3*h*h*h/20+3*h*h*r/4-4*h*r*r/3+r*r*r)*3*mass/(3*r-h)+(3/4*(h-2*r)*(h-2*r)/(3*r-h))*(3/4*(h-2*r)*(h-2*r)/(3*r-h))}" iyz="0.0"
+        izz="${mass*h/(10*(3*r-h))*(3*h*h-15*h*r+20*r*r)}" />
+      <origin></origin>
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="hollow_spherical_cap_inertial" params="r h t mass *origin">
+    <inertial>
+      <mass value="${mass}" />
+      <xacro:insert_block name="origin" />
+      <inertia ixx="${mass/20*(20*h*h*h*r-10*h*h*h*t-60*h*h*r*r+30*h*h*r*t+120*h*r*r*r-120*h*r*r*t+60*h*r*t*t-15*h*t*t*t-60*r*r*r*t+100*r*r*t*t-65*r*t*t*t+16*t*t*t*t)/(2*t*t*-3*r*t-3*h*t+6*h*r)+(3/4*(t-2*r)*(2*r*r-4*r*r+2*r*t-t*t)/(6*r*r-3*r*t-3*r*t+2*t*t))*(3/4*(t-2*r)*(2*r*r-4*r*r+2*r*t-t*t)/(6*r*r-3*r*t-3*r*t+2*t*t))}" ixy="0.0" ixz="0.0"
+        iyy="${mass/20*(20*h*h*h*r-10*h*h*h*t-60*h*h*r*r+30*h*h*r*t+120*h*r*r*r-120*h*r*r*t+60*h*r*t*t-15*h*t*t*t-60*r*r*r*t+100*r*r*t*t-65*r*t*t*t+16*t*t*t*t)/(2*t*t*-3*r*t-3*h*t+6*h*r)+(3/4*(t-2*r)*(2*r*r-4*r*r+2*r*t-t*t)/(6*r*r-3*r*t-3*r*t+2*t*t))*(3/4*(t-2*r)*(2*r*r-4*r*r+2*r*t-t*t)/(6*r*r-3*r*t-3*r*t+2*t*t))}" iyz="0.0"
+        izz="${mass/(10*(2*t*t-3*r*t-3*h*t+6*h*r))*(-20*h*h*h*r+10*h*h*h*t+60*h*h*r*r-30*h*h*r*t-60*h*r*r*t+60*h*r*t*t-15*h*t*t*t+20*r*r*t*t-25*r*t*t*t+8*t*t*t*t)}" />
+    </inertial>
+  </xacro:macro>
+
   <xacro:macro name="default_inertial">
     <inertial>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <mass
-        value="0.01" />
-      <inertia
-        ixx="0.001"
-        ixy="0.0"
-        ixz="0.0"
-        iyy="0.001"
-        iyz="0.0"
-        izz="0.001" />
+      <mass value="0.01"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
     </inertial>
   </xacro:macro>
 

diff --git a/templates/robot_description/robot.urdf.xacro b/templates/robot_description/robot.urdf.xacro
@@ -30,27 +30,4 @@
     sim_gazebo_classic="$(arg sim_gazebo_classic)"
     sim_gazebo="$(arg sim_gazebo)"/>
 
-  <xacro:if value="$(arg sim_gazebo_classic)">
-    <!-- Gazebo plugins -->
-    <gazebo reference="world">
-    </gazebo>
-    <gazebo>
-      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-        <parameters>$(arg simulation_controllers)</parameters>
-      </plugin>
-    </gazebo>
-  </xacro:if>
-
-  <xacro:if value="$(arg sim_gazebo)">
-    <!-- Gazebo plugins -->
-    <gazebo reference="world">
-    </gazebo>
-    <gazebo>
-      <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
-        <parameters>$(arg simulation_controllers)</parameters>
-        <controller_manager_node_name>$(arg prefix)controller_manager</controller_manager_node_name>
-      </plugin>
-    </gazebo>
-  </xacro:if>
-
 </robot>
diff --git a/templates/robot_description/robot_macro.ros2_control.xacro b/templates/robot_description/robot_macro.ros2_control.xacro
@@ -115,5 +115,28 @@
       </sensor>
     </ros2_control>
 
+    <xacro:if value="$(arg sim_gazebo_classic)">
+      <!-- Gazebo plugins -->
+      <gazebo reference="world">
+      </gazebo>
+      <gazebo>
+        <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+          <parameters>$(arg simulation_controllers)</parameters>
+        </plugin>
+      </gazebo>
+    </xacro:if>
+
+    <xacro:if value="$(arg sim_gazebo)">
+      <!-- Gazebo plugins -->
+      <gazebo reference="world">
+      </gazebo>
+      <gazebo>
+        <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+          <parameters>$(arg simulation_controllers)</parameters>
+          <controller_manager_node_name>$(arg prefix)controller_manager</controller_manager_node_name>
+        </plugin>
+      </gazebo>
+    </xacro:if>
+
   </xacro:macro>
 </robot>
diff --git a/templates/robot_description/robot_macro.xacro b/templates/robot_description/robot_macro.xacro
@@ -11,14 +11,14 @@
            to get more realistic behaviour-->
       <xacro:default_inertial/>
       <visual>
-        <origin rpy="0 0 0" xyz="0 0 0" />
+        <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.3" radius="0.3" />
 <!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/visual/base_link.dae" /> -->
         </geometry>
       </visual>
       <collision>
-        <origin rpy="0 0 0" xyz="0 0 0" />
+        <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.3" radius="0.3" />
 <!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/collision/base_link.stl" /> -->

diff --git a/templates/ros2_control/append_to_README.md b/templates/ros2_control/append_to_README.md
@@ -27,11 +27,11 @@ The general package structure is the following:
 Consult the repository and [ros2_control documentation](https://ros-controls.github.io/control.ros.org/) for more details.
 
 
-## Testing the *fake* robot using ros2_control-framework
+## Testing the *mock* robot using ros2_control-framework
 
 **ATTENTION**: if the package is not build and sourced do this first
 
-1. Start robot's hardware and load controllers (default configuration starts fake hardware)
+1. Start robot's hardware and load controllers (default configuration starts mock hardware)
    ```
    ros2 launch $PKG_NAME$ $ROBOT_NAME$.launch.py
    ```

diff --git a/templates/ros2_control/robot_ros2_control.launch.py b/templates/ros2_control/robot_ros2_control.launch.py
@@ -74,14 +74,14 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "use_mock_hardware",
             default_value="true",
-            description="Start robot with fake hardware mirroring command to its states.",
+            description="Start robot with mock hardware mirroring command to its states.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             "mock_sensor_commands",
             default_value="false",
-            description="Enable fake command interfaces for sensors used for simple simulations. \
+            description="Enable mock command interfaces for sensors used for simple simulations. \
             Used only if 'use_mock_hardware' parameter is true.",
         )
     )
@@ -160,9 +160,9 @@ def generate_launch_description():
         arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
     )
 
-    robot_controllers = [robot_controller]
+    robot_controller_names = [robot_controller]
     robot_controller_spawners = []
-    for controller in robot_controllers:
+    for controller in robot_controller_names:
         robot_controller_spawners += [
             Node(
                 package="controller_manager",
@@ -171,6 +171,17 @@ def generate_launch_description():
             )
         ]
 
+    inactive_robot_controller_names = ["add_some_controller_name"]
+    inactive_robot_controller_spawners = []
+    for controller in inactive_robot_controller_names:
+        inactive_robot_controller_spawners += [
+            Node(
+                package="controller_manager",
+                executable="spawner",
+                arguments=[controller, "-c", "/controller_manager", "--inactive"],
+            )
+        ]
+
     # Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node
     delay_joint_state_broadcaster_spawner_after_ros2_control_node = RegisterEventHandler(
         event_handler=OnProcessStart(
@@ -184,19 +195,30 @@ def generate_launch_description():
         )
     )
 
-    # Delay loading and activation of robot_controller after `joint_state_broadcaster`
+    # Delay loading and activation of robot_controller_names after `joint_state_broadcaster`
     delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = []
-    for controller in robot_controller_spawners:
+    for i, controller in enumerate(robot_controller_spawners):
         delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [
             RegisterEventHandler(
                 event_handler=OnProcessExit(
-                    target_action=joint_state_broadcaster_spawner,
-                    on_exit=[
-                        TimerAction(
-                            period=3.0,
-                            actions=[controller],
-                        ),
-                    ],
+                    target_action=robot_controller_spawners[i - 1]
+                    if i > 0
+                    else joint_state_broadcaster_spawner,
+                    on_exit=[controller],
+                )
+            )
+        ]
+
+    # Delay start of inactive_robot_controller_names after other controllers
+    delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner = []
+    for i, controller in enumerate(inactive_robot_controller_spawners):
+        delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner += [
+            RegisterEventHandler(
+                event_handler=OnProcessExit(
+                    target_action=inactive_robot_controller_spawners[i - 1]
+                    if i > 0
+                    else robot_controller_spawners[-1],
+                    on_exit=[controller],
                 )
             )
         ]
@@ -210,4 +232,5 @@ def generate_launch_description():
             delay_joint_state_broadcaster_spawner_after_ros2_control_node,
         ]
         + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner
+        + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner
     )