isStateValid

Проверяйте, допустимо ли состояние

Описание

пример

isValid = isStateValid(manipSV,state) проверки, если данное состояние или объединенная настройка, допустимо для модели робота дерева твердого тела, заданной блоком проверки допустимости состояния manipSV. Эта объектная функция проверяет на самостолкновения и столкновения со средой на основе свойств блока проверки допустимости состояния.

Примеры

свернуть все

Сгенерируйте состояния, чтобы сформировать путь, подтвердить движение между состояниями и проверку на самостолкновения и экологические столкновения с объектами в вашем мире. manipulatorStateSpace объект представляет объединенный пробел настройки вашей модели робота дерева твердого тела, и может произвести состояния, вычислить расстояния и осуществить границы состояния. manipulatorCollisionBodyValidator объект подтверждает состояние и движение на основе тел столкновения в вашей модели робота и любых препятствий в вашей среде.

Загрузите модель робота

Используйте loadrobot функционируйте, чтобы получить доступ к предопределенным моделям робота. Этот пример использует робота Quanser QArm™, и объединенные настройки заданы как векторы-строки.

robot = loadrobot("quanserQArm",DataFormat="row");
figure(Visible="on")
show(robot);
xlim([-0.5 0.5])
ylim([-0.5 0.5])
zlim([-0.25 0.75])
hold on

Figure contains an axes object. The axes object contains 16 objects of type patch, line. These objects represent world, base_link, YAW, BICEP, FOREARM, END-EFFECTOR, base_link_mesh, YAW_mesh, BICEP_mesh, FOREARM_mesh, END-EFFECTOR_mesh.

Сконфигурируйте пространство состояний и валидацию состояния

Создайте пространство состояний и утвердите блок проверки допустимости из модели робота.

ss = manipulatorStateSpace(robot);
sv = manipulatorCollisionBodyValidator(ss);

Установите расстояние валидации до 0.05, который основан на расстоянии, нормальном между двумя состояниями. Вы можете сконфигурировать блок проверки допустимости, чтобы проигнорировать сам столкновения, чтобы улучшить скорость валидации, но должны рассмотреть, имеет ли ваша модель робота соответствующий объединенный предельный набор настроек, чтобы гарантировать, что это не сталкивается с собой.

sv.ValidationDistance = 0.05;
sv.IgnoreSelfCollision = true;

Поместите объекты столкновения в среду робота. Установите Environment свойство объекта блока проверки допустимости столкновения использование массива ячеек объектов.

box = collisionBox(0.1,0.1,0.5); % XYZ Lengths
box.Pose = trvec2tform([0.2 0.2 0.5]); % XYZ Position
sphere = collisionSphere(0.25); % Radius
sphere.Pose = trvec2tform([-0.2 -0.2 0.5]); % XYZ Position
env = {box sphere};
sv.Environment = env;

Визуализируйте среду.

for i = 1:length(env)
    show(env{i})
end
view(60,10)

Figure contains an axes object. The axes object contains 18 objects of type patch, line. These objects represent world, base_link, YAW, BICEP, FOREARM, END-EFFECTOR, base_link_mesh, YAW_mesh, BICEP_mesh, FOREARM_mesh, END-EFFECTOR_mesh.

Запланируйте путь

Начните с домашней настройки как первая точка на пути.

rng(0); % Repeatable results
start = homeConfiguration(robot);
path = start;
idx = 1;

Запланируйте путь с помощью этих шагов в цикле:

  • Произведите соседнюю целевую настройку, с помощью Распределения Гаусса, путем определения стандартного отклонения для каждого угла поворота шарнира.

  • Проверяйте, допустимо ли произведенное целевое состояние.

  • Если произведенное целевое состояние допустимо, проверяйте, допустимо ли движение между состояниями и, если так, добавьте его в путь.

for i = 2:25
    goal = sampleGaussian(ss,start,0.25*ones(4,1));
    validState = isStateValid(sv,goal);
    
    if validState % If state is valid, check motion between states.
        [validMotion,~] = isMotionValid(sv,path(idx,:),goal);

        if validMotion % If motion is valid, add to path.
            path = [path; goal];
            idx = idx + 1;
        end
    end
end

Визуализируйте путь

После генерации пути допустимых движений визуализируйте движение робота. Поскольку вы произвели случайные состояния около домашней настройки, необходимо видеть, что рука перемещает ту начальную настройку.

Чтобы визуализировать путь исполнительного элемента конца в 3-D, получите преобразование относительно основной мировой системы координат в каждой точке. Сохраните точки как xyz вектор сдвига. Постройте путь исполнительного элемента конца.

eePose = nan(3,size(path,1));

for i = 1:size(path,1)
    show(robot,path(i,:),PreservePlot=false);
    eePos(i,:) = tform2trvec(getTransform(robot,path(i,:),"END-EFFECTOR")); % XYZ translation vector
    plot3(eePos(:,1),eePos(:,2),eePos(:,3),"-b",LineWidth=2)
    drawnow
end

Figure contains an axes object. The axes object contains 28 objects of type patch, line. These objects represent world, base_link, YAW, BICEP, FOREARM, END-EFFECTOR, base_link_mesh, YAW_mesh, BICEP_mesh, FOREARM_mesh, END-EFFECTOR_mesh.

Входные параметры

свернуть все

Блок проверки допустимости состояния манипулятора в виде manipulatorCollisionBodyValidator объект, который является подклассом nav.StateValidator (Navigation Toolbox). Блок проверки допустимости состояния содержит свойства, которые определяют поведение этой функции и isMotionValid.

Состояние робота на объединенном пробеле в виде n - вектор-строка элемента из объединенных положений. n является количеством нефиксированных соединений в rigidBodyTree модель робота.

Типы данных: double

Выходные аргументы

свернуть все

Валидность состояния ввода, возвращенного как логический скаляр. Состояние допустимо, если оно не приводит к самостолкновениям, столкновениям со средой, и это в границах состояния.

Типы данных: логический

Введенный в R2021b
Для просмотра документации необходимо авторизоваться на сайте