Robótica

Clase 14

Semana 17 - 20/08/2025

Ejercicio 1 - URDF

Debe estar parametrizado al menos el radio de las ruedas de tracción y la separación de las mismas

<?xml version="1.0"?>
<robot name="diffbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- ... -->
    <xacro:property name="wheel_radius" value="0.035"/>
    <xacro:property name="wheel_sep" value="0.135"/>
    <!-- ... -->
    <link name="left_wheel">
        <!-- ... -->
        <collision>
            <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
            <geometry>
                <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
            </geometry>
        </collision>

        <xacro:inertial_cylinder mass="${wheel_mass}" length="${wheel_width}" radius="${wheel_radius}">
            <origin xyz="0 0 0" rpy="${pi/2.0} 0 0"/>
        </xacro:inertial_cylinder>
    </link>
</robot>

Declaración de parámetros en el URDF no utilizados No aprovecha las constantes definidas por XACRO

Ejercicio 1 y 2 - Dependencias

description.launch.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import Command, PathJoinSubstitution, LaunchConfiguration, EqualsSubstitution

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    # Ubicación del paquete y del archivo URDF
    urdf_path = PathJoinSubstitution(
        [FindPackageShare("diffbot_description"), "urdf", "diffbot_description.urdf.xacro"]
    )
    
    # Procesar archivo URDF
    urdf = Command(['xacro ', urdf_path])

    # Publicar el 'robot description'
    node_robot_state_publisher = Node(
        package = 'robot_state_publisher',
        executable = 'robot_state_publisher',
        output = 'screen',
        parameters=[{
            'robot_description': urdf
        }]
    )

    # Parámetro para ejecutar el 'joint_state_publisher_gui' 
    testing_ = DeclareLaunchArgument(
        'testing', default_value='true'
    )
    
    node_joint_state_publisher_gui = Node(
        condition = IfCondition(
            EqualsSubstitution(LaunchConfiguration('testing'), 'true')
        ),
        package = 'joint_state_publisher_gui',
        executable = 'joint_state_publisher_gui',
        output = 'screen'
    )

    # RViz
    node_rviz2 = Node(
        condition=IfCondition(
            EqualsSubstitution(LaunchConfiguration('testing'), 'true')
        ),
        package = 'rviz2',
        executable = 'rviz2',
    )

    return LaunchDescription([
        testing_,
        node_robot_state_publisher,
        node_joint_state_publisher_gui,
        node_rviz2,
    ])

Ejercicio 1 y 2 - Dependencias

Dependencias: xacro, robot_state_publisher, joint_state_publisher, rviz2

<!-- ... -->
<package format="3">
    <name>diffbot_description</name>
    <version> .. </version>
    <description> .. </description>

    <!-- ... -->
    <exec_depend>xacro</exec_depend>
    <exec_depend>robot_state_publisher</exec_depend>

    <exec_depend>joint_state_publisher_gui</exec_depend>
    <exec_depend>rviz2</exec_depend>
    <!-- ... -->
</package>

Dependencias no utilizadas por el paquete

Ejercicio 3 - Gazebo

• Dependencias

• ros_gz_bridge y use_sim_time

• Carga del robot:

gazebo.launch.py

    # ...
    spawn_entity = Node(
        package="ros_gz_sim",
        executable="create",
        arguments=[
            "-entity", "diffbot",
            "-topic", "robot_description",
            # "-x", "0.0",
            # "-y", "0.0",
            "-z", "0.05",   # Al menos media rueda por encima del suelo
            # "-Y", "0.0",
        ],
        output="screen",
    )
    # ...

• Dependencias no utilizadas por el paquete

• Uso de ‘ros_gz_bridge’ no solicitado

• Parámetros adicionales no solicitados, ej ‘use_sim_time’

• El robot inicia a la altura z=0 con referencia al base link

Ejercicio 4a - ros2_control

Editar el archivo de definición del robot [..] de forma tal que las juntas correspondientes a las ruedas de tracción reciban comandos de velocidad y devuelvan el estado de posición y velocidad

ros2_control.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
    </hardware>
    <joint name="front_left_wheel_joint">
      <command_interface name="velocity" />
      
      <state_interface name="position" />
      <state_interface name="velocity" />
    </joint>
    <joint name="front_right_wheel_joint">
      <command_interface name="velocity" />
      
      <state_interface name="velocity" />
      <state_interface name="position" />
    </joint>
    <joint name="caster_rotation_joint">
      <state_interface name="position" />
    </joint>
    <joint name="caster_wheel_joint">
      <state_interface name="position" />
    </joint>
  </ros2_control>

  <gazebo>
    <plugin filename="gz_ros2_control-system"
      name="gz_ros2_control::GazeboSimROS2ControlPlugin">
      <parameters>
        $(find diffbot_control)/config/controllers.yaml
      </parameters>
    </plugin>
  </gazebo>
</robot>

• Faltan dependencias para el paquete ‘robot_control’

• No respeta estructura solicitada

• Límites de velocidad a las ruedas no solicitado

• Configuración de interfaces de estado incorrecta para las juntas

Ejercicio 4b - ros2_control

Crear un paquete [..] con los parámetros necesarios para que el controller_manager de ROS2 control cargue un JointStateBroadcaster y dos controladores de velocidad de tipo JointGroupVelocityController, uno para cada rueda de tracción.

controllers.yaml

controller_manager:
  ros__parameters:
    update_rate: 30

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    
    left_wheel_velocity_controller:
      type: velocity_controllers/JointGroupVelocityController
    
    right_wheel_velocity_controller:
      type: velocity_controllers/JointGroupVelocityController

left_wheel_velocity_controller:
  ros__parameters:
    joints:
      - front_left_wheel_joint

    command_interfaces:
      - velocity

    state_interfaces:
      - position
      - velocity

right_wheel_velocity_controller:
  ros__parameters:
    joints:
      - front_right_wheel_joint

    command_interfaces:
      - velocity

    state_interfaces:
      - position
      - velocity

• La configuración de los controladores no responde a la consigna

Ejercicio 4c - ros2_control

Editar el archivo launch del ejercicio 3 para invocar al comando load_controller de ros2_control y cargar los controladores del inciso anterior.

gazebo.launch.py

    # ...
    load_joint_state_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', '--use-sim-time', 
             'joint_state_broadcaster'],
        output='screen'
    )
    load_left_wheel_velocity_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', '--use-sim-time', 
             'left_wheel_velocity_controller'],
        output='screen'
    )
    load_right_wheel_velocity_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', '--use-sim-time',
             'right_wheel_velocity_controller'],
        output='screen'
    )
    # ...

• El archivo ‘launch’ debía ser el mismo que el ejercicio 3

• Delay con acciones de tipo ‘timer’ no solicitadas

• No sigue la consigna respecto a la inicialización de los controladores

Ejercicio 5

Calcular la velocidad lineal y angular del robot y de las ruedas para que se complete:

• una trayectoria recta de 1[m] en 10 [s].
• una trayectoria circular con un radio de 0.5 [m] en sentido horario en 20 [s].

Datos del anexo

• Radio de las ruedas: \(0.035 \textrm{[m]}\)
• Separación de las ruedas: \(0.135 \textrm{[m]}\)

Ejercicio 5 - Trayectoria recta

Una trayectoria recta de 1[m] en 10 [s].

• V lineal del robot: \(v = \frac{1 \mathrm{[m]}}{10 \mathrm{[s]}} = 0.1 \mathrm{\left[\frac{m}{s}\right]}\)

• V angular del robot: \(\dot{\theta} = 0\) (linea recta)

• V lineal rueda derecha = V lineal rueda izquierda (linea recta): \[\upsilon_{L|R} = 0.1 \mathrm{\left[\frac{m}{s}\right]}\]

• V angular rueda derecha = V angular rueda izquierda: \[\dot\phi_{L|R} = \frac{v}{r} = \frac{0.1 \mathrm{\left[\frac{m}{s}\right]}}{0.035 \mathrm{\left[\frac{m}{rad}\right]}} \approx 2.857 \mathrm{\left[\frac{rad}{s}\right]}\]

Ejercicio 5 - Trayectoria circular

Una trayectoria circular con un radio de 0.5 [m] en sentido horario en 20 [s].

• V angular del robot: \(\require{color} \dot{\theta} = \textcolor{Maroon}{-} \frac{2 \pi \mathrm{[rad]}}{20 \mathrm{[s]}} \approx \textcolor{Maroon}{-}0.314 \mathrm{\left[\frac{rad}{s}\right]}\)

• V lineal del robot: \(v = \dot{\theta} \times \mathcal{R} = \textcolor{Maroon}{-} 0.314 \mathrm{\left[\frac{rad}{s}\right]} \times \textcolor{Maroon}{-} 0.5 \mathrm{\left[\frac{m}{rad}\right]} = 0.157 \mathrm{\left[\frac{m}{s}\right]}\)

• V lineal rueda izquierda: \(\upsilon_L = \dot{\theta}(\mathcal{R}-\frac{b}{2}) = -0.314 (-0.5 - \frac{0.135}{2}) \approx 0.178 \mathrm{\left[\frac{m}{s}\right]}\)

• V lineal rueda derecha: \(\upsilon_R = \dot{\theta}(\mathcal{R}+\frac{b}{2}) = -0.314 (-0.5 + \frac{0.135}{2}) \approx 0.135 \mathrm{\left[\frac{m}{s}\right]}\)

• V angular rueda izquierda: \(\dot\phi_{L} = \frac{\upsilon_L}{r} = \frac{0.178 \mathrm{\left[\frac{m}{s}\right]}}{0.035 \mathrm{\left[\frac{m}{rad}\right]}} \approx 5.09 \mathrm{\left[\frac{rad}{s}\right]}\)

• V angular rueda derecha: \(\dot\phi_{R} = \frac{\upsilon_R}{r} = \frac{0.136 \mathrm{\left[\frac{m}{s}\right]}}{0.035 \mathrm{\left[\frac{m}{rad}\right]}} \approx 3.88 \mathrm{\left[\frac{rad}{s}\right]}\)

Ejercicio 6

Examinar la definición de los mensajes de tipo geometry_msgs/Twist y describir cuál sería la secuencia de comandos de velocidad a aplicar al robot para seguir la trayectoria mostrada en la Figura 1 utilizando dichos mensajes. La velocidad máxima de giro de los motores es de 50 [rpm]

Ejercicio 6

Trayectoria A: punto inicial \(\blacksquare\) y punto final \(\bullet\)

• Giro antihorario con \(\mathcal{R} = 0.25 \mathrm{[m]}\)
• Linea recta \(1 \mathrm{[m]}\)
• Giro horario con \(\mathcal{R} = 0.25 \mathrm{[m]}\)
• Linea recta \(1 \mathrm{[m]}\)

Trayectoria B: punto inicial \(\bullet\) y punto final \(\blacksquare\):

• Linea recta \(1 \mathrm{[m]}\)
• Giro antihorario con \(\mathcal{R} = 0.25 \mathrm{[m]}\)
• Linea recta \(1 \mathrm{[m]}\)
• Giro horario con \(\mathcal{R} = 0.25 \mathrm{[m]}\)

Ejercicio 6 - Segmento recto

• Por el Ejercicio 5, la velocidad de rotación de las ruedas para una velocidad lineal de \(0.1 \mathrm{\left[\frac{m}{s}\right]}\) y una velocidad angular de \(0 \mathrm{\left[\frac{rad}{s}\right]}\) es aproximadamente \(2.86 \mathrm{\left[\frac{rad}{s}\right]}\):

\[ 2.86 \mathrm{\left[\frac{rad}{s}\right]} \approx 0.455 \mathrm{[rps]} \approx 27.3 \mathrm{[rpm]} < 50 \mathrm{[rpm]}\]

Por lo tanto: se propone un comando de \(v = 0.1 \mathrm{\left[\frac{m}{s}\right]}\) y \(\dot\theta = 0 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

Ejercicio 6 - Giro horario

• Por el Ejercicio 5, la velocidad de rotación de las ruedas para una velocidad angular de \(-0.314 \mathrm{\left[\frac{rad}{s}\right]}\) y una velocidad lineal de \(\textcolor{Maroon}{-} 0.314 \mathrm{\left[\frac{rad}{s}\right]} \times \textcolor{Maroon}{-} 0.25 \mathrm{\left[\frac{m}{rad}\right]} = 0.0785 \mathrm{\left[\frac{m}{s}\right]}\) es:

\[\dot\phi_{L} = \frac{\dot{x} - \frac{b}{2} \dot\theta}{r} = \frac{0.0785 - \frac{0.135}{2} (-0.314) \mathrm{\left[\frac{m}{s}\right]}}{0.035 \mathrm{\left[\frac{m}{rad}\right]}} \approx 2.848 \mathrm{\left[\frac{rad}{s}\right]}\]

\[\dot\phi_{R} = \frac{\dot{x} + \frac{b}{2} \dot\theta}{r} = \frac{0.0785 + \frac{0.135}{2} (-0.314) \mathrm{\left[\frac{m}{s}\right]}}{0.035 \mathrm{\left[\frac{m}{rad}\right]}} \approx 1.637 \mathrm{\left[\frac{rad}{s}\right]}\]

\(2.848 \mathrm{\left[\frac{rad}{s}\right]} \approx 27.2 \mathrm{[rpm]} < 50 \mathrm{[rpm]}\) y \(1.637 \mathrm{\left[\frac{rad}{s}\right]} \approx 15.6 \mathrm{[rpm]} < 50 \mathrm{[rpm]}\)

Por lo tanto: se propone un comando de \(\dot\theta = -0.314 \mathrm{\left[\frac{rad}{s}\right]}\) y \(v = 0.0785 \mathrm{\left[\frac{m}{s}\right]}\) por 10 [s]

Ejercicio 6 - Giro anti-horario

• Utilizando la misma magnitud que el giro horario, las velocidades angulares de las ruedas son las mismas pero intercambiadas: \(\dot\theta = 0.314 \mathrm{\left[\frac{rad}{s}\right]}\) y \(v = 0.0785 \mathrm{\left[\frac{m}{s}\right]}\)

\[\dot\phi_{L} = \frac{\dot{x} - \frac{b}{2} \dot\theta}{r} = \frac{0.0785 - \frac{0.135}{2} (0.314) \mathrm{\left[\frac{m}{s}\right]}}{0.035 \mathrm{\left[\frac{m}{rad}\right]}} \approx 1.637 \mathrm{\left[\frac{rad}{s}\right]}\]

\[\dot\phi_{R} = \frac{\dot{x} + \frac{b}{2} \dot\theta}{r} = \frac{0.0785 + \frac{0.135}{2} (0.314) \mathrm{\left[\frac{m}{s}\right]}}{0.035 \mathrm{\left[\frac{m}{rad}\right]}} \approx 2.848 \mathrm{\left[\frac{rad}{s}\right]}\]

Por lo tanto: se propone un comando de \(\dot\theta = -0.314 \mathrm{\left[\frac{rad}{s}\right]}\) y \(v = 0.0785 \mathrm{\left[\frac{m}{s}\right]}\) por 10 [s]

Ejercicio 6 - geometry_msgs/Twist

• Definición del mensaje

    geometry_msgs/Twist
    ├── geometry_msgs/Vector3 linear
    |   ├── float64 x     ⬅️
    |   ├── float64 y
    |   └── float64 z
    └── geometry_msgs/Vector3 angular
        ├── float64 x
        ├── float64 y
        └── float64 z     ⬅️

Ejercicio 6 - Respuesta

Trayectoria A: punto inicial \(\blacksquare\) y punto final \(\bullet\)

• Giro antihorario: \(v = 0.0785 \mathrm{\left[\frac{m}{s}\right]}\) y \(\dot\theta = 0.314 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

{linear: { x: 0.0785 }, angular: { z: 0.314 }}

• Linea recta: \(v = 0.1 \mathrm{\left[\frac{m}{s}\right]}\) y \(\dot\theta = 0 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

{linear: { x: 0.1 }, angular: { z: 0.0 }}

• Giro horario: \(v = 0.0785 \mathrm{\left[\frac{m}{s}\right]}\) y \(\dot\theta = -0.314 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

{linear: { x: 0.0785 }, angular: { z: -0.314 }}

• Linea recta: \(v = 0.1 \mathrm{\left[\frac{m}{s}\right]}\) y \(\dot\theta = 0 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

{linear: { x: 0.1 }, angular: { z: 0.0 }}

Ejercicio 6 - Respuesta

Trayectoria B: punto inicial \(\bullet\) y punto final \(\blacksquare\):

• Linea recta: \(v = 0.1 \mathrm{\left[\frac{m}{s}\right]}\) y \(\dot\theta = 0 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

{linear: { x: 0.1 }, angular: { z: 0.0 }}

• Giro antihorario: \(v = 0.0785 \mathrm{\left[\frac{m}{s}\right]}\) y \(\dot\theta = 0.314 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

{linear: { x: 0.0785 }, angular: { z: 0.314 }}

• Linea recta: \(v = 0.1 \mathrm{\left[\frac{m}{s}\right]}\) y \(\dot\theta = 0 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

{linear: { x: 0.1 }, angular: { z: 0.0 }}

• Giro horario: \(v = 0.0785 \mathrm{\left[\frac{m}{s}\right]}\) y \(\dot\theta = -0.314 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

{linear: { x: 0.0785 }, angular: { z: -0.314 }}

Ejercicio 7

Examinar la definición de los mensajes del topic suscripto por el JointGroupVelocityController. Calcule las velocidades angulares de las ruedas para cada comando del ejercicio 6 y construya la secuencia de mensajes de comando correspondientes.

• Segmento recto:

\(\dot\phi_{L|R} = 2.857 \mathrm{\left[\frac{rad}{s}\right]}\)

• Giro horario:

\(\dot\phi_{L} = 2.848 \mathrm{\left[\frac{rad}{s}\right]}\), \(\dot\phi_{R} = 1.637 \mathrm{\left[\frac{rad}{s}\right]}\)

• Giro anti-horario:

\(\dot\phi_{L} = 1.637 \mathrm{\left[\frac{rad}{s}\right]}\), \(\dot\phi_{R} = 2.848 \mathrm{\left[\frac{rad}{s}\right]}\)

Ejercicio 7 - std_msgs/Float64MultiArray

• Definición del mensaje

    std_msgs/Float64MultiArray
    ├── std_msgs/MultiArrayLayout layout
    └── float64[] data    ⬅️

Ejercicio 7 - Respuesta

Trayectoria A: punto inicial \(\blacksquare\) y punto final \(\bullet\)

• Giro antihorario: \(\dot\phi_{L} = 1.637 \mathrm{\left[\frac{rad}{s}\right]}\) y \(\dot\phi_{R} = 2.848 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

ros2 topic pub /left_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [1.64]}

ros2 topic pub /right_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [2.85]}

• Linea recta: \(\dot\phi_{L|R} = 2.857 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

ros2 topic pub /left_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [2.86]}

ros2 topic pub /right_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [2.86]}

• Giro horario: \(\dot\phi_{L} = 2.848 \mathrm{\left[\frac{rad}{s}\right]}\), \(\dot\phi_{R} = 1.637 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

ros2 topic pub /left_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [2.85]}

ros2 topic pub /right_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [1.64]}

• Linea recta: \(\dot\phi_{L|R} = 2.857 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

ros2 topic pub /left_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [2.86]}

ros2 topic pub /right_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [2.86]}

Ejercicio 7 - Respuesta

Trayectoria B: punto inicial \(\bullet\) y punto final \(\blacksquare\)

• Linea recta: \(\dot\phi_{L|R} = 2.857 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

ros2 topic pub /left_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [2.86]}

ros2 topic pub /right_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [2.86]}

• Giro antihorario: \(\dot\phi_{L} = 1.637 \mathrm{\left[\frac{rad}{s}\right]}\) y \(\dot\phi_{R} = 2.848 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

ros2 topic pub /left_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [1.64]}

ros2 topic pub /right_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [2.85]}

• Linea recta: \(\dot\phi_{L|R} = 2.857 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

ros2 topic pub /left_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [2.86]}

ros2 topic pub /right_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [2.86]}

• Giro horario: \(\dot\phi_{L} = 2.848 \mathrm{\left[\frac{rad}{s}\right]}\), \(\dot\phi_{R} = 1.637 \mathrm{\left[\frac{rad}{s}\right]}\) por 10 [s]

ros2 topic pub /left_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [2.85]}

ros2 topic pub /right_wheel_velocity_controller/commands std_msgs/msg/Float64MultiArray {data: [1.64]}

Ejercicio 8 - Cinemática inversa

Dentro del paquete (*)_control, cree un nodo que reciba comandos de tipo geometry_msgs/Twist a través de un topic llamado cmd_vel [..]

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

class DiffbotControl(Node):
    def __init__(self):
        super().__init__('diffbot_control_node')
        
        # Creación de suscriptor
        self.sub = self.create_subscription(Twist, 'cmd_vel', self.sub_callback, 10)

        
    def sub_callback(self, msg: Twist):
        # ...

def main(args=None):
    # 1. Inicialización
    rclpy.init(args=args)
    # 2. Creación de nodo
    nodo = DiffbotControl()
    try:
        # 3. Procesamiento de mensajes y callback
        rclpy.spin(nodo)
    else:
        # 4. Finalización 
        rclpy.shutdown()

if __name__ == '__main__':
    main()

• No respeta nombres solicitados
• Estructura incompleta para la función ‘main’

Ejercicio 8 - Cinemática inversa

[..] calcule las velocidades angulares en base al modelo cinemático inverso del robot y escriba los comandos de velocidad de cada rueda de tracción en los topics left_wheel_cmd y right_wheel_cmd [..]

Modelo cinemático inverso de un robot diferencial

\[ \begin{cases} \textcolor{Plum}{\dot\phi_R} = \frac{1}{r} (\textcolor{ForestGreen}{\dot{x}} + \frac{\textcolor{Gray}{b}}{2} \textcolor{Orange}{\dot\theta}) \\[0.5em] \textcolor{Plum}{\dot\phi_L} = \frac{1}{r} (\textcolor{ForestGreen}{\dot{x}} - \frac{\textcolor{Gray}{b}}{2} \textcolor{Orange}{\dot\theta}) \end{cases} \]

• Modelo cinemático mal aplicado, calcula las velocidades lineales de las ruedas, no las angulares
• No respeta nombre de topics solicitados

Ejercicio 8 - Cinemática inversa

[..] calcule las velocidades angulares en base al modelo cinemático inverso del robot y escriba los comandos de velocidad de cada rueda de tracción en los topics left_wheel_cmd y right_wheel_cmd [..]

# ..
from std_msgs.msg import Float64MultiArray

class DiffbotControl(Node):
    def __init__(self):
        # ..

        # Crear los dos publisher a los topics de cada rueda
        self.pub_lwheel = self.create_publisher(Float64MultiArray,
            'left_wheel_velocity_controller/commands', 10)
        self.pub_rwheel = self.create_publisher(Float64MultiArray,
            'right_wheel_velocity_controller/commands', 10)
        
    def sub_callback(self, msg: Twist):
        # Obtengo la velocidad lineal y angular deseada
        x_dot = msg.linear.x
        w_dot = msg.angular.z
        
        # Modelo cinemático inverso
        phi_dot_lwheel = (x_dot - ((self.wheel_sep/2) * w_dot)) / self.wheel_r
        phi_dot_rwheel = (x_dot + ((self.wheel_sep/2) * w_dot)) / self.wheel_r

        # Crear los mensajes y publicar
        lwheel_msg = Float64MultiArray()
        lwheel_msg.data = [phi_dot_lwheel]
        self.pub_lwheel.publish(lwheel_msg)

        rwheel_msg = Float64MultiArray()
        rwheel_msg.data = [phi_dot_rwheel]
        self.pub_rwheel.publish(rwheel_msg)

# ..

• Modelo cinemático mal aplicado, calcula las velocidades lineales de las ruedas, no las angulares
• No respeta nombre de topics solicitados

Ejercicio 8 - Cinemática inversa

[..] Tenga en cuenta que los parámetros del robot se encuentran en el robot description.

# ..
from std_msgs.msg import Float64MultiArray

class DiffbotControl(Node):
    def __init__(self):
        # ..

        # Parámetro de separación de ruedas
        self.declare_parameter('wheel_separation', 0.135)
        # Parámetro de radio de rueda
        self.declare_parameter('wheel_radius', 0.07/2)

        # Obtener los parámetros
        self.wheel_sep = self.get_parameter('wheel_separation').get_parameter_value().double_value
        self.wheel_r = self.get_parameter('wheel_radius').get_parameter_value().double_value

        # ..

    def sub_callback(self, msg: Twist):
        # ...

def main(args=None):
    # 1. Inicialización
    rclpy.init(args=args)
    # 2. Creación de nodo
    nodo = DiffbotControl()
    try:
        # 3. Procesamiento de mensajes y callback
        rclpy.spin(nodo)
    else:
        # 4. Finalización 
        rclpy.shutdown()

if __name__ == '__main__':
    main()

• Parámetros geométricos del robot programados dentro de la clase

Ejercicio 9 - Odometría

Crear un nodo que lea el o los topics de posición de las juntas de las ruedas (publicados por el JointStateBroadcaster) [..] El nodo debe recibir como parámetro las dimensiones geométricas del modelo cinemático y los nombres correspondientes a las juntas de las ruedas de tracción.

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState

class DiffbotOdometry(Node):
    def __init__(self):
        super().__init__('diffbot_odometry_node')

        # Parámetro de radio y separación de ruedas
        self.declare_parameter('wheel_separation', 0.135)
        self.declare_parameter('wheel_radius', 0.07/2)

        # Parámetro para el nomrbe de las juntas
        self.declare_parameter('left_wheel_name', 'front_left_wheel_joint')
        self.declare_parameter('right_wheel_name', 'front_right_wheel_joint')
               
        # Crear el suscriptor al topic joint_states
        self.sub = self.create_subscription(JointState, 'joint_states', self.sub_callback, 10)

        # ..
        
    def sub_callback(self, msg: Twist):
        # ...

# ..

• Estructura incompleta para la función ‘main’

Ejercicio 9 - Odometría

[..] calcule la odometría basado en el modelo cinemático directo del robot y publique la misma en el topic \odom con mensajes de tipo nav_msgs/Odometry. [..]

# ..
from nav_msgs.msg import Odometry

class DiffbotOdometry(Node):
    def __init__(self):
        super().__init__('diffbot_odometry_node')

        # Crear el publisher de la odometría
        self.pub_odom = self.create_publisher(Odometry, 'odom', 10)

        # ..
        
    def sub_callback(self, msg: Twist):
        # Chequear si el primer elemento es el joint
        # de la rueda izquierda o derecha
        lwheel_ang, rwheel_ang = 0.0, 0.0
        for name, position in zip(msg.name, msg.position):
            if name == self.left_wheel_name:
                lwheel_ang = position
            if name == self.right_wheel_name:
                rwheel_ang = position
        
        # Cálculo de la distancia recorrida
        dl_k = (lwheel_ang - self.lwheel_ang_old) * self.wheel_r
        dr_k = (rwheel_ang - self.rwheel_ang_old) * self.wheel_r
        
        dA_k = (dr_k + dl_k) / 2
        Dw_k = (dr_k - dl_k) / self.wheel_sep

        # Cálculo de la odometría (pose)
        x_k_new = self.x_k + dA_k * np.cos(self.w_k)
        y_k_new = self.y_k + dA_k * np.sin(self.w_k)
        w_k_new = self.w_k + Dw_k

        odom_msg = Odometry()
        odom_msg.pose.pose.position.x = x_k_new
        odom_msg.pose.pose.position.y = y_k_new
        
        odom_msg.pose.pose.orientation.x = 0.0
        odom_msg.pose.pose.orientation.y = 0.0
        odom_msg.pose.pose.orientation.z = np.sin(w_k_new/2)
        odom_msg.pose.pose.orientation.w = np.cos(w_k_new/2)
        
        # Publicar
        self.pub_odom.publish(odom_msg)

        # Actualizar valores
        self.lwheel_ang_old = lwheel_ang
        self.rwheel_ang_old = rwheel_ang
        self.x_k = x_k_new
        self.y_k = y_k_new
        self.w_k = w_k_new

# ..

• La actualización del ángulo ‘theta’ está en el orden incorrecto
• No respeta campos solicitados para el mensaje ‘odom’
• El modelo de odometría no es el solicitado

Ejercicio 10

Cree un nuevo paquete (*)_bringup con un archivo launch en el cual se cargue todo el sistema desarrollado [..]

bringup.launch.py

# ..
def generate_launch_description():
    # Launch simulation
    simulation = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare('diffbot_gazebo'), 'launch', 'gazebo.launch.py']
            ),
        ),
    )

    node_diffbot_controller = Node(
        package = 'diffbot_control',
        executable = 'run_controller',
        output = 'screen',
    )
    
    node_diffbot_odometry = Node(
        package = 'diffbot_control',
        executable = 'run_odometry',
        output = 'screen',
    )

    return LaunchDescription([
        simulation,
        node_diffbot_controller,
        node_diffbot_odometry,
    ])

• Dependencias no utilizadas por el paquete
• No incluye los archivos ‘launch’ ya desarrollados
• Parámetros adicionales no solicitados, ej ‘use_sim_time’
• Nodos adicionales no solicitados, ej ‘go_to_pose_controller_node’ o ‘ros_gz_bridge’
• Incluye un ejecutable incorrecto ‘odometry_publisher’

Ejercicio 11 - tf2

(Opción B) Modificar el nodo del ejercicio 9 para que que publique la transformación entre los frames odom y base_link. Agregar un parámetro para que cuando no se requiera esta transformación, no se publique.

# ..
class DiffbotOdometry(Node):
    def __init__(self):
        # ..      
        # Parámetro para activar la publicación de las transformaciones
        self.declare_parameter('publish_tf', True)

        # ..
        self.publish_tf = self.get_parameter('publish_tf').get_parameter_value().bool_value
        if self.publish_tf:
          # Inicializar el broadcaster
          self.tf_broadcaster = TransformBroadcaster(self)
        # ..

    def sub_callback(self, msg: Twist):
      # ..
      if self.publish_tf:
        self.send_tf()
        
    def send_tf(self):
        tf = TransformStamped()

        tf.header.stamp = self.get_clock().now().to_msg()
        # Marco de referencia (padre)
        tf.header.frame_id = 'odom'  
        # Marco objetivo (hijo)                      
        tf.child_frame_id = 'base_link'                 

        # Traslación
        tf.transform.translation.x = self.x_k
        tf.transform.translation.y = self.y_k
        tf.transform.translation.z = 0.0

        # Rotación
        tf.transform.rotation.x = 0.0
        tf.transform.rotation.y = 0.0
        tf.transform.rotation.z = np.sin(self.w_k/2)
        tf.transform.rotation.w = np.cos(self.w_k/2)
        
        # Enviar la transformación
        self.tf_broadcaster.sendTransform(tf)

# ..

• Se supone elección de la Opcion B, pero no agrega parámetro para desactivar transformación
• Cálculo de cuaternion en 3D no solicitado

Ejercicio 12

Cree un archivo launch en el paquete (*)_bringup que extienda (inclusión o copia) al del ejercicio 10 y añada la ejecución de RViz junto con su respectivo archivo de configuración, el cual muestre el robot en el frame odom y las transformaciones de todos los links definidos en el URDF.

odom_tf.launch.py

# ..
def generate_launch_description():
    # Launch simulation
    simulation = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare('diffbot_gazebo'), 'launch', 'gazebo.launch.py']
            ),
        ),
    )

    node_diffbot_controller = Node(
        package = 'diffbot_control',
        executable = 'run_controller',
        output = 'screen',
    )
    
    node_diffbot_odometry = Node(
        package = 'diffbot_control',
        executable = 'run_odometry',
        output = 'screen',
        parameters = [{ 'publish_tf': True }]
    )
    
    node_rviz =  Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', PathJoinSubstitution(
                [FindPackageShare('diffbot_bringup'), 'rviz', 'odom.rviz']
            )
        ]
    )

    return LaunchDescription([
        simulation,
        node_diffbot_controller,
        node_diffbot_odometry,
        node_odom_view,
        node_odom_view_gt,
        node_rviz
    ])

# ..

• Parámetros adicionales no solicitados, ej ‘use_sim_time’
• Delay con acciones de tipo ‘timer’ no solicitadas