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