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)];

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

[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')