역기구학을 사용하여 2차원 경로 추적하기

소개

이 예제는 inverseKinematics 클래스를 사용하여 단순한 2차원 매니퓰레이터의 역기구학을 계산하는 방법을 보여줍니다. 이 매니퓰레이터 로봇은 회전 조인트를 가진 단순한 2자유도 평면 매니퓰레이터로, 여러 강체를 rigidBodyTree 객체로 조립하여 만듭니다. 원형 궤적이 2차원 평면에서 생성되어 역기구학 솔버에 점으로 제공됩니다. 솔버는 이 궤적을 구현하기 위한 조인트 위치를 계산합니다. 마지막으로, 원형 궤적을 구현하는 로봇 컨피규레이션을 애니메이션으로 표시합니다.

로봇 만들기

rigidBodyTree 객체를 만들고 강체와 강체에 연결된 조인트를 만듭니다. 각 강체의 기하 속성을 지정하고 로봇에 추가합니다.

빈 강체 트리 모델을 만드는 것부터 시작합니다.

robot = rigidBodyTree('DataFormat','column','MaxNumBodies',3);

로봇 팔의 길이를 지정합니다.

L1 = 0.3;
L2 = 0.3;

'joint1' 조인트가 있는 'link1' 바디를 추가합니다.

body = rigidBody('link1');
joint = rigidBodyJoint('joint1', 'revolute');
setFixedTransform(joint,trvec2tform([0 0 0]));
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'base');

'joint2' 조인트가 있는 'link2' 바디를 추가합니다.

body = rigidBody('link2');
joint = rigidBodyJoint('joint2','revolute');
setFixedTransform(joint, trvec2tform([L1,0,0]));
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'link1');

'fix1' 고정 조인트가 있는 'tool' 엔드 이펙터를 추가합니다.

body = rigidBody('tool');
joint = rigidBodyJoint('fix1','fixed');
setFixedTransform(joint, trvec2tform([L2, 0, 0]));
body.Joint = joint;
addBody(robot, body, 'link2');

로봇의 세부 정보를 표시하여 입력 속성의 유효성을 검사합니다. 로봇에는 2개의 비고정 조인트가 필요합니다. 링크 바디는 회전 조인트를 통해 부모 바디에 연결되고 엔드 이펙터는 고정 조인트를 통해 부모 바디에 연결됩니다.

showdetails(robot)

--------------------
Robot: (3 bodies)

 Idx    Body Name    Joint Name    Joint Type    Parent Name(Idx)   Children Name(s)
 ---    ---------    ----------    ----------    ----------------   ----------------
   1        link1        joint1      revolute             base(0)   link2(2)  
   2        link2        joint2      revolute            link1(1)   tool(3)  
   3         tool          fix1         fixed            link2(2)   
--------------------

궤적 정의하기

10초 동안 추적할 원을 정의합니다. 이 원은 반지름이 0.15이고 xy 평면에 있습니다.

t = (0:0.2:10)'; % Time
count = length(t);
center = [0.3 0.1 0];
radius = 0.15;
theta = t*(2*pi/t(end));
points = center + radius*[cos(theta) sin(theta) zeros(size(theta))];

역기구학 해

inverseKinematics 객체를 사용하여 궤적을 따라 지정된 엔드 이펙터 위치를 구현하는 로봇 컨피규레이션의 해를 구합니다.

컨피규레이션 해를 행렬 qs로 사전 할당합니다.

q0 = homeConfiguration(robot);
ndof = length(q0);
qs = zeros(count, ndof);

역기구학 솔버를 만듭니다. 이 워크플로에서는 엔드 이펙터 자세에 중요한 유일한 요인이 xy 카테시안 점이므로 weight 벡터의 네 번째와 다섯 번째 요소에 0이 아닌 가중치를 지정합니다. 나머지 요소는 모두 0으로 설정합니다.

ik = inverseKinematics('RigidBodyTree', robot);
weights = [0, 0, 0, 1, 1, 0];
endEffector = 'tool';

점의 궤적을 순환하면서 원을 추적합니다. 각 점마다 ik 객체를 호출하여 엔드 이펙터 위치를 구현하는 조인트 컨피규레이션을 생성합니다. 나중에 사용할 수 있도록 컨피규레이션을 저장합니다.

qInitial = q0; % Use home configuration as the initial guess
for i = 1:count
    % Solve for the configuration satisfying the desired end effector
    % position
    point = points(i,:);
    qSol = ik(endEffector,trvec2tform(point),weights,qInitial);
    % Store the configuration
    qs(i,:) = qSol;
    % Start from prior solution
    qInitial = qSol;
end

해를 애니메이션으로 보기

해의 각 프레임마다 해당 로봇 컨피규레이션을 사용하여 로봇을 플로팅합니다. 또한 목표 궤적도 플로팅합니다.

궤적의 첫 번째 컨피규레이션에서의 로봇을 표시합니다. 플롯을 조정하여 원이 그려지는 2차원 평면을 표시합니다. 목표 궤적을 플로팅합니다.

figure
show(robot,qs(1,:)');
view(2)
ax = gca;
ax.Projection = 'orthographic';
hold on
plot(points(:,1),points(:,2),'k')
axis([-0.1 0.7 -0.3 0.5])

rateControl 객체를 설정하여 초당 15프레임의 고정 레이트로 로봇 궤적을 표시합니다. 역기구학 솔버의 각 컨피규레이션에서의 로봇을 표시합니다. 팔이 원형 궤적을 추적하며 움직이는 모습을 관찰합니다.

framesPerSecond = 15;
r = rateControl(framesPerSecond);
for i = 1:count
    show(robot,qs(i,:)','PreservePlot',false);
    drawnow
    waitfor(r);
end

참고 항목

rigidBodyTree | rigidBody | rigidBodyJoint | inverseKinematics