여러 기구학적 제약 조건을 가진 도달 궤적 계획하기

라이브 스크립트 열기

이 예제에서는 일반화된 역기구학을 사용하여 로봇 매니퓰레이터의 조인트 공간 궤적을 계획하는 방법을 보여줍니다. 여러 제약 조건을 결합하여 그리퍼가 테이블 위에 놓인 컵으로 다가가는 궤적을 생성합니다. 이러한 제약 조건은 그리퍼의 자세를 미리 결정하지 않고도 그리퍼가 컵에 직선으로 접근하고 테이블로부터 안전한 거리를 유지하도록 합니다.

로봇 모델 설정하기

이 예제에서는 7자유도 로봇 매니퓰레이터인 KUKA LBR iiwa 모델을 사용합니다. importrobot은 URDF(Unified Robot Description Format) 파일에 저장된 설명으로부터 rigidBodyTree 모델을 생성합니다.

lbr = importrobot('iiwa14.urdf'); % 14 kg payload version
lbr.DataFormat = 'row';
gripper = 'iiwa_link_ee_kuka';

컵의 치수를 정의합니다.

cupHeight = 0.2;
cupRadius = 0.05;
cupPosition = [-0.5, 0.5, cupHeight/2];

컵의 중심을 나타내는 고정 바디를 로봇 모델에 추가합니다.

body = rigidBody('cupFrame');
setFixedTransform(body.Joint, trvec2tform(cupPosition))
addBody(lbr, body, lbr.BaseName);

계획 문제 정의하기

이 예제의 목표는 다음 기준을 충족하는 로봇 컨피규레이션 시퀀스를 생성하는 것입니다.

홈 컨피규레이션에서 시작함
로봇 컨피규레이션이 급격하게 변화하지 않음
그리퍼를 "테이블" 위에서 최소 5cm 거리 유지(z=0)
그리퍼가 컵에 다가갈 때 컵과 정렬되어야 함
그리퍼가 컵 중심에서 5cm 떨어진 위치에서 종료함

이 예제에서는 constraint 객체를 활용하여 위의 기준을 만족하는 로봇 컨피규레이션을 생성합니다. 생성되는 궤적은 5개의 컨피규레이션 웨이포인트로 구성됩니다. 첫 번째 웨이포인트 q0은 홈 컨피규레이션으로 설정됩니다. repmat를 사용하여 나머지 컨피규레이션을 qWaypoints에 사전 할당합니다.

numWaypoints = 5;
q0 = homeConfiguration(lbr);
qWaypoints = repmat(q0, numWaypoints, 1);

다음 제약 조건 입력값을 받는 generalizedInverseKinematics 솔버를 생성합니다.

카테시안 범위 - 그리퍼의 높이를 제한함
위치 목표 - 컵의 위치를 그리퍼를 기준으로 지정함.
조준 제약 조건 - 그리퍼를 컵의 축과 정렬시킴
방향 목표 - 그리퍼가 컵에 접근하는 동안 고정된 방향을 유지함
조인트 위치 범위 - 웨이포인트 사이에서 조인트 위치의 변화를 제한함.

gik = generalizedInverseKinematics('RigidBodyTree', lbr, ...
    'ConstraintInputs', {'cartesian','position','aiming','orientation','joint'})

gik = 
  generalizedInverseKinematics with properties:

      NumConstraints: 5
    ConstraintInputs: {'cartesian'  'position'  'aiming'  'orientation'  'joint'}
       RigidBodyTree: [1x1 rigidBodyTree]
     SolverAlgorithm: 'BFGSGradientProjection'
    SolverParameters: [1x1 struct]

constraint 객체 만들기

솔버에 입력값으로 전달되는 constraint 객체를 만듭니다. 이러한 객체는 각 제약 조건에 필요한 파라미터를 포함합니다. 필요하면 솔버 호출 사이에 이들 파라미터를 수정합니다.

그리퍼를 테이블 위(음의 z 방향)에서 최소 5cm 거리를 유지시키기 위한 카테시안 범위 제약 조건을 만듭니다. 다른 모든 값은 inf 또는 -inf로 주어집니다.

heightAboveTable = constraintCartesianBounds(gripper);
heightAboveTable.Bounds = [-inf, inf; ...
                           -inf, inf; ...
                           0.05, inf]

heightAboveTable = 
  constraintCartesianBounds with properties:

        EndEffector: 'iiwa_link_ee_kuka'
      ReferenceBody: ''
    TargetTransform: [4x4 double]
             Bounds: [3x2 double]
            Weights: [1 1 1]

그리퍼를 기준으로 한 컵 위치의 제약 조건을 만듭니다(허용오차는 5mm임).

distanceFromCup = constraintPositionTarget('cupFrame');
distanceFromCup.ReferenceBody = gripper;
distanceFromCup.PositionTolerance = 0.005

distanceFromCup = 
  constraintPositionTarget with properties:

          EndEffector: 'cupFrame'
        ReferenceBody: 'iiwa_link_ee_kuka'
       TargetPosition: [0 0 0]
    PositionTolerance: 0.0050
              Weights: 1

목표를 로봇보다 훨씬 높은 위치에 놓아서 iiwa_link_ee 프레임의 z축이 대략 수직이 되도록 하는 조준 제약 조건을 만듭니다. iiwa_link_ee 프레임은 이 제약 조건에 따라 그리퍼가 컵의 축과 정렬하도록 방향을 맞춥니다.

alignWithCup = constraintAiming('iiwa_link_ee');
alignWithCup.TargetPoint = [0, 0, 100]

alignWithCup = 
  constraintAiming with properties:

         EndEffector: 'iiwa_link_ee'
       ReferenceBody: ''
         TargetPoint: [0 0 100]
    AngularTolerance: 0
             Weights: 1

조인트 위치 범위에 대한 제약 조건을 만듭니다. 이 제약 조건의 Bounds 속성을 이전 컨피규레이션을 기준으로 설정해서 조인트 위치의 변화를 제한합니다.

limitJointChange = constraintJointBounds(lbr)

limitJointChange = 
  constraintJointBounds with properties:

     Bounds: [7x2 double]
    Weights: [1 1 1 1 1 1 1]

그리퍼에 대해 허용오차가 1도인 방향 제약 조건을 만듭니다. 이 제약 조건으로 인해 그리퍼의 방향은 TargetOrientation 속성에 지정된 값과 일치해야 합니다. 이 제약 조건을 사용하여 그리퍼가 컵에 최종 접근하는 동안 그리퍼의 방향을 고정합니다.

fixOrientation = constraintOrientationTarget(gripper);
fixOrientation.OrientationTolerance = deg2rad(1)

fixOrientation = 
  constraintOrientationTarget with properties:

             EndEffector: 'iiwa_link_ee_kuka'
           ReferenceBody: ''
       TargetOrientation: [1 0 0 0]
    OrientationTolerance: 0.0175
                 Weights: 1

컵을 가리키는 컨피규레이션 구하기

이 컨피규레이션은 그리퍼를 컵에서 일정 거리 떨어진 위치에 놓아서 그리퍼가 최종 접근할 때 적절하게 정렬되도록 해야 합니다.

intermediateDistance = 0.3;

constraint 객체에는 솔버가 충돌하는 제약 조건을 처리하는 방법을 결정하는 Weights 속성이 있습니다. 제약 조건의 가중치를 0으로 설정하면 제약 조건이 비활성화됩니다. 이 컨피규레이션에서는 조인트 위치 범위와 방향 제약 조건을 비활성화합니다.

limitJointChange.Weights = zeros(size(limitJointChange.Weights));
fixOrientation.Weights = 0;

그리퍼 프레임에서 컵의 목표 위치를 설정합니다. 컵은 그리퍼의 z축 위의 지정된 거리에 놓여야 합니다.

distanceFromCup.TargetPosition = [0,0,intermediateDistance];

gik 솔버를 사용하여 입력 제약 조건을 만족하는 로봇 컨피규레이션을 구합니다. 모든 입력 제약 조건을 지정해야 합니다. 이 컨피규레이션을 두 번째 웨이포인트로 설정합니다.

[qWaypoints(2,:),solutionInfo] = gik(q0, heightAboveTable, ...
                       distanceFromCup, alignWithCup, fixOrientation, ...
                       limitJointChange);

그리퍼를 컵까지 직선으로 움직이는 컨피규레이션 구하기

조인트 위치 범위와 방향 제약 조건을 다시 활성화합니다.

limitJointChange.Weights = ones(size(limitJointChange.Weights));
fixOrientation.Weights = 1;

방향 제약 조건은 컵과 정렬하는 제약 조건과 중복되므로 컵 정렬 제약 조건을 비활성화합니다.

alignWithCup.Weights = 0;

방향 제약 조건을 이전 컨피규레이션(qWaypoints(2,:))을 기준으로 설정하여 방향을 유지합니다. 그리퍼에서 로봇 모델의 베이스까지의 변환을 구합니다. 동차 변환을 쿼터니언으로 변환합니다.

fixOrientation.TargetOrientation = ...
    tform2quat(getTransform(lbr,qWaypoints(2,:),gripper));

각 웨이포인트에 컵과 그리퍼 사이의 거리를 정의합니다.

finalDistanceFromCup = 0.05;
distanceFromCupValues = linspace(intermediateDistance, finalDistanceFromCup, numWaypoints-1);

각 웨이포인트 사이의 조인트 위치에 허용되는 최대 변화를 정의합니다.

maxJointChange = deg2rad(10);

남은 각 웨이포인트에 대해 솔버를 호출합니다.

for k = 3:numWaypoints
    % Update the target position.
    distanceFromCup.TargetPosition(3) = distanceFromCupValues(k-1);
    % Restrict the joint positions to lie close to their previous values.
    limitJointChange.Bounds = [qWaypoints(k-1,:)' - maxJointChange, ...
                               qWaypoints(k-1,:)' + maxJointChange];
    % Solve for a configuration and add it to the waypoints array.
    [qWaypoints(k,:),solutionInfo] = gik(qWaypoints(k-1,:), ...
                                         heightAboveTable, ...
                                         distanceFromCup, alignWithCup, ...
                                         fixOrientation, limitJointChange);
end

생성된 궤적 시각화하기

매끄러운 궤적을 생성하기 위해 웨이포인트 사이를 보간합니다. pchip 함수를 사용하여 로봇의 조인트 제한을 위반할 수 있는 오버슈트를 방지합니다.

framerate = 15;
r = rateControl(framerate);
tFinal = 10;
tWaypoints = [0,linspace(tFinal/2,tFinal,size(qWaypoints,1)-1)];
numFrames = tFinal*framerate;
qInterp = pchip(tWaypoints,qWaypoints',linspace(0,tFinal,numFrames))';

보간된 각 컨피규레이션에서 그리퍼 위치를 계산합니다.

gripperPosition = zeros(numFrames,3);
for k = 1:numFrames
    gripperPosition(k,:) = tform2trvec(getTransform(lbr,qInterp(k,:), ...
                                                    gripper));
end

초기 컨피규레이션의 로봇을 테이블 및 컵과 함께 표시합니다.

figure;
show(lbr, qWaypoints(1,:), 'PreservePlot', false);
hold on
exampleHelperPlotCupAndTable(cupHeight, cupRadius, cupPosition);
p = plot3(gripperPosition(1,1), gripperPosition(1,2), gripperPosition(1,3));

매니퓰레이터를 애니메이션으로 표시하고 그리퍼 위치를 플로팅합니다.

hold on
for k = 1:size(qInterp,1)
    show(lbr, qInterp(k,:), 'PreservePlot', false);
    p.XData(k) = gripperPosition(k,1);
    p.YData(k) = gripperPosition(k,2);
    p.ZData(k) = gripperPosition(k,3);
    waitfor(r);
end
hold off

생성된 컨피규레이션을 나중에 사용할 수 있도록 MAT 파일에 저장하려면 다음을 실행하십시오.

>> save('lbr_trajectory.mat', 'tWaypoints', 'qWaypoints');