Вопрос или проблема
Итак, я пытаюсь импортировать четырехногого робота с рукой в систему Drake. У робота есть файлы URDF и mesh (формат ROS), которые я преобразовал, чтобы они импортировались правильно, с добавленным тегом transmission в файл URDF, который отсутствовал.
Я пытаюсь запустить пример базового контроллера PIDController, но сейчас я застрял, так как не все суставы имеют актуатор, но PIDController ожидает, что размер входного вектора будет равен 2 * размеру выходного вектора. Количество актуаторов составляет 14, так что PIDController ожидает 28, но у системы всего state_output_port() 47 (так или иначе простое число?). Интуитивно, я полагаю, что должен получить Позицию/Скорость этих 14 суставов с актуаторами на них, но я испытываю трудности с тем, как это сделать?
model = parser.AddModelsFromUrl(xgo_mini)
plant: MultibodyPlant # содержит модель
plant.get_actuation_input_port().size() == len(plant.GetActuatedJointIndicies(model)
# размер равен 14, как и ожидалось: 3 актуатора на ногу + 2 для руки = 3*4 + 2 = 14
plant.get_state_output_port().size()
# Это 47... как-то.
controller: PiDController # инициализирован с 14 параметрами Kp/Kd/Ki
builder: DiagramBuilder
builder.Connect(plant.get_state_output_port(), controller.get_input_port_estimated_state()
RuntimeError: DiagramBuilder::Connect: Несоответствие размеров векторов при подключении выходного порта состояния системы plant (размер 47) к входному порту estimated_state системы controller (размер 28)
Оригинальный файл URDF взят от этого робота (но модифицирован для Drake) https://github.com/sskorol/xgo_ros Спасибо заранее
Ответ или решение
Получение положения/скорости только актуированных соединений из MultiBodyPlant в Drake
При работе с многотелевыми системами в библиотеке Drake, такими как ваш квадрупед, каждый совместимый элемент, включая соединения и приводы, может потребовать отдельного подхода. Ваша задача заключается в извлечении данных о положении и скорости только для тех соединений, которые имеют актуаторы.
Чтобы решить возникшую проблему, следуйте следующим шагам:
1. Изучите структуру состояния системы
Система MultiBodyPlant возвращает общее состояние, которое включает в себя не только информацию об актуированных, но и об неактуированных соединениях. В вашем случае общее количество выходных данных состояния составляет 47, что, вероятно, объясняется наличием дополнительных соединений (например, свободных или фиксированных) в вашей модели.
2. Используйте индексы актуированных соединений
Для получения состояния только актуированных соединений, вам нужно использовать метод GetActuatedJointIndices():
actuated_indices = plant.GetActuatedJointIndices()Этот метод поможет извлечь индексы соединений, которые имеют приводы, и, следовательно, нуждаются в контроле.
3. Извлечение позиции и скорости
Используя индексы актуированных соединений, вы теперь можете извлечь соответствующее положение и скорость. Это делается следующим образом:
# Получаем общее состояние системы
state = plant.get_state_output_port().Eval(context)
# Получаем позиции и скорости для актуированных соединений
positions = state[np.concatenate([2 * actuated_indices, 2 * actuated_indices + 1])]
velocities = state[np.concatenate([2 * actuated_indices + len(plant.GetPositions()), 2 * actuated_indices + 1 + len(plant.GetVelocities())])]В приведенном выше коде state представляет собой массив, который содержит как позиционные, так и скорости соединений. Данные индексы позволяют извлекать информацию только для актуированных соединений.
4. Подключение к PIDController
После извлечения нужных вам значений, вы можете передать их в PIDController:
# Подключаем выход состояния вашего растения к контроллеру
builder.Connect(plant.get_state_output_port().get_center_port(), controller.get_input_port_estimated_state())Обратите внимание, что необходимо убедиться в корректности подключений: ожидаемые размеры входных и выходных портов должны совпадать.
Заключение
Следуя этим шагам, вы сможете изолировать данные лишь для актуированных соединений в MultiBodyPlant, что решит проблему несовпадения размеров между выходным портом состояния и входным портом контроллера. Если у вас возникнут дополнительные вопросы, не стесняйтесь обратиться за помощью к сообществу Drake или исследовать документацию.
SEO элементы:
- Ключевые слова: Drake, MultiBodyPlant, Quadrupedal robot, PIDController, State output port, Actuated joints.
- Мета описание: Узнайте, как получить положения и скорости только актуированных соединений из MultiBodyPlant в Drake, решая проблемы несовпадения размеров для PIDController.