Main Content

Kinova Gen3 매니퓰레이터를 사용하여 작업-공간 궤적과 조인트-공간 궤적을 계획하고 실행하기

이 예제는 엔드 이펙터의 초기 자세에서 원하는 자세로 이동하도록 보간된 조인트 궤적을 생성하고 시뮬레이션하는 방법을 보여줍니다. 궤적의 시간 설정은 대략적으로 원하는 EOAT(end of arm tool) 속도에 기반합니다.

Kinova® Gen3 강체 트리(RBT) 로봇 모델을 불러옵니다.

robot = loadrobot('kinovaGen3','DataFormat','row','Gravity',[0 0 -9.81]);

현재의 로봇 조인트 컨피규레이션을 설정합니다.

currentRobotJConfig = homeConfiguration(robot);

조인트 개수를 구하고 엔드 이펙터 RBT 프레임을 지정합니다.

numJoints = numel(currentRobotJConfig);
endEffector = "EndEffector_Link";

궤적 시간 스텝과 대략적으로 원하는 툴 속도를 지정합니다.

timeStep = 0.1; % seconds
toolSpeed = 0.1; % m/s

초기 엔드 이펙터 자세와 최종 엔드 이펙터 자세를 설정합니다.

jointInit = currentRobotJConfig;
taskInit = getTransform(robot,jointInit,endEffector);

taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);

작업-공간 궤적 생성하기

보간을 통해 작업-공간 궤적 웨이포인트를 계산합니다.

먼저 툴의 이동 거리를 계산합니다.

distance = norm(tform2trvec(taskInit)-tform2trvec(taskFinal));

다음으로 이동 거리와 원하는 툴 속도를 기반으로 궤적 시간을 정의합니다.

initTime = 0;
finalTime = (distance/toolSpeed) - initTime;
trajTimes = initTime:timeStep:finalTime;
timeInterval = [trajTimes(1); trajTimes(end)];

taskInittaskFinal 사이를 보간하여 중간 작업-공간 웨이포인트를 계산합니다.

[taskWaypoints,taskVelocities] = transformtraj(taskInit,taskFinal,timeInterval,trajTimes); 

작업-공간 모션 제어하기

조인트에 대한 PD 제어를 위해 작업 공간 모션 모델을 생성합니다. taskSpaceMotionModel 객체는 작업-공간 비례-미분 제어에서 강체 트리 모델의 모션을 모델링합니다.

tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','EndEffector_Link');

제어된 동작이 기준 위치만 따르도록 방향에 대한 비례 이득과 미분 이득을 0으로 설정합니다.

tsMotionModel.Kp(1:3,1:3) = 0;
tsMotionModel.Kd(1:3,1:3) = 0;

초기 상태(조인트 위치와 속도)를 정의합니다.

q0 = currentRobotJConfig; 
qd0 = zeros(size(q0));

ode15s를 사용하여 로봇 모션을 시뮬레이션합니다. 이 문제의 경우 폐루프 시스템은 경직성 문제입니다. 즉, 문제의 어느 부분에서 스케일링에 차이가 있다는 뜻입니다. 따라서 적분기는 때때로 매우 작은 스텝을 취해야 하며, ode45와 같은 비경직성 ODE 솔버는 동일한 문제를 해결하는 데 훨씬 더 오랜 시간이 걸립니다. 자세한 내용은 문서에서 ODE 솔버 선택하기 항목과 경직성(Stiff) ODE 풀기 항목을 참조하십시오.

기준 상태는 매 순간 변하기 때문에 보간된 궤적 입력을 매 순간 상태 도함수로 업데이트하려면 래퍼 함수가 필요합니다. 따라서 예제 헬퍼 함수는 ODE 솔버에 함수 핸들로 전달됩니다. 그 결과 매니퓰레이터 상태는 stateTask에 출력됩니다.

[tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state),timeInterval,[q0; qd0]);

조인트-공간 궤적 생성하기

로봇에 대한 inverse kinematics 객체를 생성합니다.

ik = inverseKinematics('RigidBodyTree',robot);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1 1 1 1 1 1];

역기구학을 사용하여 초기 조인트 컨피규레이션과 원하는 조인트 컨피규레이션을 계산합니다. 보간이 최소 영역에 있도록 값을 pi로 래핑합니다.

initialGuess = jointInit;
jointFinal = ik(endEffector,taskFinal,weights,initialGuess);

기본적으로 IK 해는 조인트 제한을 준수합니다. 그러나 연속 조인트(무한한 범위의 회전 조인트)의 경우 결과 값이 불필요하게 클 수 있으며, 최종 궤적이 최소 거리이도록 [-pi, pi]로 래핑할 수 있습니다. Gen3의 비연속 조인트는 이미 이 간격 내로 제한되어 있기 때문에 조인트 값을 간단하게 pi로 래핑하면 충분합니다. 연속 조인트는 간격 [-pi, pi]에 매핑되고 나머지 값은 그대로 유지됩니다.

wrappedJointFinal = wrapToPi(jointFinal);

3차 다항식 함수를 사용하여 이 사이를 보간하여 균일한 간격의 조인트 컨피규레이션으로 구성된 배열을 생성합니다.

ctrlpoints = [jointInit',wrappedJointFinal'];
jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes);

조인트-공간 궤적 제어하기

조인트에 대한 PD 제어를 위해 조인트-공간 모션 모델을 생성합니다. jointSpaceMotionModel 객체는 강체 트리 모델의 모션을 모델링하고 지정된 조인트 위치에 비례-미분 제어를 사용합니다.

jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');

초기 상태(조인트 위치와 속도)를 설정합니다.

q0 = currentRobotJConfig; 
qd0 = zeros(size(q0));

ode15s를 사용하여 로봇 모션을 시뮬레이션합니다. 이번에도 예제 헬퍼 함수는 매 순간 기준 입력을 업데이트하기 위해 ODE 솔버에 대한 함수 핸들 입력값으로 사용됩니다. 조인트-공간 상태는 stateJoint에 출력됩니다.

[tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state),timeInterval,[q0; qd0]);

로봇 궤적 시각화 및 비교하기

로봇의 초기 컨피규레이션을 표시합니다.

show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off');
hold on
axis([-1 1 -1 1 -0.1 1.5]);

작업 공간 궤적을 시각화합니다. stateTask 상태를 처음부터 끝까지 반복하고 현재 시간을 기준으로 보간합니다.

for i=1:length(trajTimes)
    % Current time 
    tNow= trajTimes(i);
    % Interpolate simulated joint positions to get configuration at current time
    configNow = interp1(tTask,stateTask(:,1:numJoints),tNow);
    poseNow = getTransform(robot,configNow,endEffector);
    show(robot,configNow,'PreservePlot',false,'Frames','off');
    taskSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'b.','MarkerSize',20);
    drawnow;
end

조인트-공간 궤적을 시각화합니다. jointTask 상태를 처음부터 끝까지 반복하고 현재 시간을 기준으로 보간합니다.

% Return to initial configuration
show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off');

for i=1:length(trajTimes)
    % Current time 
    tNow= trajTimes(i);
    % Interpolate simulated joint positions to get configuration at current time
    configNow = interp1(tJoint,stateJoint(:,1:numJoints),tNow);
    poseNow = getTransform(robot,configNow,endEffector);
    show(robot,configNow,'PreservePlot',false,'Frames','off');
    jointSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'r.','MarkerSize',20);
    drawnow;
end

% Add a legend and title
legend([taskSpaceMarker jointSpaceMarker], {'Defined in Task-Space', 'Defined in Joint-Space'});
title('Manipulator Trajectories')

Figure contains an axes object. The axes object with title Manipulator Trajectories, xlabel X, ylabel Y contains 150 objects of type line, patch. These objects represent Defined in Task-Space, base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh, Defined in Joint-Space.