locomotion_controller

Данный модуль является контроллером шагания робота. В качестве алгоритма стабилизации корпуса он использует метод ZMP Preview Control.

На вход алгоритма поступают желаемые скорости движения, на основе которых блок Foot Planner строит желаемые точки опоры робота, а также выдает информацию о том, в какой фазе нога должна находиться в каждый момент времени: опоры или переноса. Если нога должна находиться в фазе переноса, то модуль Swing Trajectory Generater формирует траекторию на основе текущего положения ступни, высоты шага, частоты шага и точки, в которую нужно поставить ногу. Если нога должна находиться в фазе опоры, то модуль ZMP Trajectory Generator формирует задающую траекторию движения точки ZMP (Zero-Moment Point), которая постуупает на блок ZMP Previe Control. Этот модуль, используя модель линейного обратного маятника генерирует задающую траекторию для ЦМ (центра масс) корпуса робота. Далее, благодаря модулю Body Moving Control координаты ЦМ преобразуются в координаты ступней модуля. Выходы модулей Swing Trajectory Generator и Body Moving Control передаются на модуль мультиплексирующий модуль Mux, который выдает желаемые координаты ступней каждой ноги робота в зависимости от того, в какой фазе должна находиться конкретная нога. В конце, блок решения обратной задачи кинематики IK Solver преобразует координаты в желаемые углы в сочленениях и отправляет их в аппаратную часть по протоколу LCM.

Алгоритм способен обеспечивать передвижение робота, используя три кинематические схемы ног робота:

  • как у млекопитающих, обозначается буквой "m"

  • X-образная, обозначается буквой "х"

  • О-обращная, обозначается буквой "о"

LCM API

  • Канал SERVO_CMD: предназначен для отправки задающих параметров сервоприводов. Формат сообщения имеет следующий вид:

struct servo_cmd_msg
{
    float timestamp; // временная метка

    float position[12];  // угловое положение [рад]
    float velocity[12];  // угловая скорость [рад/сек]
    float torque[12];  // момент [Н м]
    float kp[12];  // пропорциональный коэффициент
    float kd[12];  // дифференциальный коэффициент
}

  • Канал SERVO_STATE: предназначен для получения состояния сервоприводов. Формат сообщения имеет следующий вид:

struct servo_state_msg
{
    float timestamp; // временная метка

    float position[12];  // угловое положение [рад]
    float velocity[12];  // угловая скорость [рад/сек]
    float torque[12];  // момент [Н м]
}

ROS API

Параметры

Файл параметров располагается в папке config

  • general

    • frequency (по умолчанию: 300) - частота, на которой работает модуль

    • contact_flags - если True, то показывает находится ли в данный момент нога в фазе опоры

  • simulation - параметры для симуляции

    • kp - пропорциональный коэффициент для ПД-регулятора привода

    • kd - дифференциальный коэффициент для ПД-регулятора привода

    • yaw_kp - пропорциональный коэффициент для П-регулятора, стабилизирующего курс робота

    • kinematic_scheme - выбор кинематической схемы ног робота. Доступные варианты: "m", "x", "o". То, как будет выглядеть робот в стойке, показано на рисунках выше.

    • robot_height - высота робота в стойке

    • cog_x_offset - смещение центра тяжести корпуса робота по оси X

    • cof_z_offset - смещение центра тяжести корпуса робота по оси Y

    • ef_init_x - изначальное положение стопы робота по оси X

    • ef_init_y - изначальное положение стопы робота по оси Y

    • stride_frequency - частота шага робота во время ходьбы

    • preview_horizon - на сколько времени вперед алгоритм прогнозирует будущее состояние робота

  • hardware

    • kp - пропорциональный коэффициент для ПД-регулятора привода

    • kd - дифференциальный коэффициент для ПД-регулятора привода

    • yaw_kp - пропорциональный коэффициент для П-регулятора, стабилизирующего курс робота

    • kinematic_scheme - выбор кинематической схемы ног робота. Доступные варианты: "m", "x", "o". То, как будет выглядеть робот в стойке, показано на рисунках выше.

    • robot_height - высота робота в стойке

    • cog_x_offset - смещение центра тяжести корпуса робота по оси X

    • cof_z_offset - смещение центра тяжести корпуса робота по оси Y

    • ef_init_x - изначальное положение стопы робота по оси X

    • ef_init_y - изначальное положение стопы робота по оси Y

    • stride_frequency - частота шага робота во время ходьбы

    • preview_horizon - на сколько времени вперед алгоритм прогнозирует будущее состояние робота

  • topics - названия ros-топиков и lcm-каналов, с которыми работает пакет

    • ros_servo_cmd_topic (по умолчанию: "joint_cmd") - топик, по которому передаются команды на приводы

    • lcm_servo_cmd_channel (по умолчанию: "SERVO_CMD") - название LCM-канала, по которому передаются команды на приводы

    • lcm_servo_state_channel (по умолчанию: "SERVO_STATE") - название LCM-канала, по которому передается состояние приводов

    • ros_imu_topic (по умолчанию: "imu/data") - название топика для получения данных с IMU

    • ros_cmd_vel_topic (по умолчанию: "cmd_vel") - топик для получения желаемой скорости

    • ros_cmd_pose_topic (по умолчанию: "cmd_pose") - топик для получения желаемого положения и ориентации корпуса робота

    • ros_cmd_ef_pose_topic (по умолчанию: "ef_position/command") - топик для получения желаемого положения стоп робота

    • ros_cmd_joint_pos_topic (по умолчанию: "joint_group_position_controller/command") - топик для получения желаемых значений для сервоприводов

    • ros_state_contact_flags_topic (по умолчанию: "foot_contacts") - топик для выдачи информации о том, находится ли нога в фазе опоры

    • ros_state_ef_pose_topic (по умолчанию: "ef_position/states") - топик для выдачи текущего положения стоп робота

Входящие топики

  • imu/data (sensor_msgs/Imu) - данные с IMU (линейное ускорение, угловая скорость, ориентация)

  • cmd_vel (geometry_msgs/Twist) - желаемая скорость перемещения робота

  • cmd_pose (geometry_msgs/Twist) - желаемые положения и ориентация корпуса робота

  • ef_position/command (geometry_msgs/PoseArray) - желаемые положения стоп робота

  • disable (std_msgs/Bool) - если True, то нода блокирует свою работу

Исходящие топики

Previousjoy_converterNextlog_writer

Last updated