本实例为如何生成和模拟插值关节轨迹，从一个初始运动到一个理想的末端执行器姿态。

轨迹的定时是基于手臂工具(EOAT)的一个近似的期望末端速度。

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

在taskkinit和taskFinal之间插值，计算中间笛卡尔空间的轨迹点

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

控制笛卡尔空间运动

建立关节空间运动模型，对关节进行PD控制。taskSpaceMotionModel对象对刚体树模型在任务空间比例微分控制下的运动进行建模。

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

将方向上的比例增益和导数增益设置为零，这样受控行为就会遵循参考位置:

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

定义初始状态(关节位置和速度)

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

使用ode15s来模拟机器人的运动。

对于这个问题，闭环系统是刚性的，这意味着在这个问题的某个地方存在缩放上的差异。

因此，积分器有时被迫采取非常小的步骤，而像ode45这样的非刚性ODE求解器将需要更长的时间来解决相同的问题。

由于参考状态在每一时刻都会发生变化，因此需要一个包装器函数来将插值后的轨迹输入更新为每一时刻的状态导数。

因此，一个示例helper函数作为函数句柄传递给ODE求解器。最终机械手状态输出到stateTask中。

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

生成关节空间轨迹

为机器人创建一个逆运动学对象

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

在它们之间使用三次多项式函数插值来生成均匀间隔关节配置的数组。

使用b样条曲线生成平滑的轨迹。

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

关节空间轨迹控制

建立关节空间运动模型，对关节进行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 timeconfigNow = 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 timeconfigNow = 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')