使用第3代KINOVA机械臂计划和执行任务 - 与关节空间轨迹
该示例示出如何生成和模拟内插的关节的轨迹来移动从初始到期望的端部执行器的姿势。轨迹的定时是基于臂工具（EOAT）速度的近似所需的最终。
加载KINOVA 3代刚体树（RBT）的机器人模型。
机器人= loadrobot（'kinovaGen3'，“DataFormat”，'行'，'重力'[0 0 -9.81]);
设置当前机器人关节配置。
currentRobotJConfig = homeConfiguration(机器人);
得到关节数目和末端执行器RBT框架。
numJoints = numel（currentRobotJConfig）;末端执行器=“EndEffector_Link”;
指定轨迹时间步长，近似所期望的工具速度。
时间步长= 0.1;％秒toolSpeed = 0.1;％ 多发性硬化症
将最初和最后的末端执行器的姿态。
jointInit = currentRobotJConfig;taskInit = getTransform（机器人，jointInit，末端执行器）;taskFinal = trvec2tform（[0.4,0,0.6]）* axang2tform（[0 1 0π）;
生成任务空间轨迹
通过插值计算任务空间轨迹路径点。
首先，计算工具行驶距离。
距离=规范(tform2trvec (taskInit) -tform2trvec (taskFinal));
接下来，根据行驶距离和所需工具的速度定义轨迹倍。
initTime = 0;finalTime =（距离/ toolSpeed） - initTime;trajTimes = initTime：TIMESTEP：finalTime;一个时间间隔= [trajTimes（1）;trajTimes（端）];
之间插入taskInit和taskFinal计算中间的任务空间的航点。
[taskWaypoints, taskVelocities] = transformtraj (taskInit、taskFinal timeInterval, trajTimes);
控制工作空间运动
创建对关节的PD控制关节空间运动模型。该taskSpaceMotionModel对象模型任务空间比例 - 微分控制下的刚体树模型的运动。
tsMotionModel = taskSpaceMotionModel ('RigidBodyTree'，机器人，“EndEffectorName”，“EndEffector_Link”);
将方向上的比例增益和导数增益设置为零，使受控行为仅遵循参考位置:
tsMotionModel.Kp (1:3, 1:3) = 0;tsMotionModel.Kd (1:3, 1:3) = 0;
定义初始状态(关节位置和速度)。
q0 = currentRobotJConfig;qd0 = 0(大小(q0));
使用ode15s模拟机器人的运动。由于基准状态在每个时刻改变，需要一个包装函数在每个时刻更新的轨迹输入到状态衍生物插值。因此，一个例子辅助函数作为函数句柄ODE求解器通过。将所得的操纵器的状态是在输出stateTask。
[tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state)，timeInterval，[q0;qd0]);
产生关节间隙轨迹
为机器人创建一个逆运动学对象。
本土知识= inverseKinematics ('RigidBodyTree'，机器人）;ik.SolverParameters.AllowRandomRestart = FALSE;权重= [1 1 1 1 1 1];
使用逆运动学计算初始和期望的关节构型。
initialGuess = wrapToPi（jointInit）;jointFinal = IK（末端执行器，taskFinal，重量，initialGuess）;jointFinal = wrapToPi（jointFinal）;
在它们之间内插使用三次多项式函数来生成均匀间隔的连接结构的阵列。使用B样条，产生平滑的轨迹。
ctrlpoints = [jointInit, jointFinal];jointConfigArray = cubicpolytraj (ctrlpoints timeInterval trajTimes);jointWaypoints = bsplinepolytraj (jointConfigArray timeInterval 1);
控制联合空间轨迹
创建对关节的PD控制关节空间运动模型。该jointSpaceMotionModel对象模型刚性体树模型和用途比例 - 微分控制在指定的关节位置的运动。
jsMotionModel = jointSpaceMotionModel（'RigidBodyTree'，机器人，“移动型”，“PDControl”);
设置初始状态（关节位置和速度）。
q0 = currentRobotJConfig;qd0 = 0(大小(q0));
使用ode15s模拟机器人的运动。同样，一个示例帮助函数被用作ODE求解器的函数句柄输入，以便在每个瞬间及时更新引用输入。联合空间状态被输出进来stateJoint。
[tJoint，stateJoint] = ode15s（@（T，状态）exampleHelperTimeBasedJointInputs（jsMotionModel，一个时间间隔，jointConfigArray，T，状态），一个时间间隔，[Q0; QD0]）;
可视化和比较机器人轨迹
显示机器人的初始配置。
表演(currentRobotJConfig的机器人“PreservePlot”假的,“框架”，“关闭”);持有在轴([-1 -1 -1 -0.1 1.5]);
可视化任务空间轨迹。迭代通过stateTask状态和内插根据当前时间。
对于I = 1：长度（trajTimes）％ 当前时间TNOW = trajTimes（ⅰ）;%插入模拟关节位置，以获得当前时刻的配置configNow = interp1 (tTask stateTask (: 1: numJoints), tNow);poseNow = getTransform(机器人,configNow, endEffector);显示（机器人，configNow，“PreservePlot”假的,“框架”，“关闭”);plot3（poseNow（1,4），poseNow（2,4），poseNow（3,4），'B'。，“MarkerSize”，20）的DrawNow;结束
可视化的关节空间轨迹。迭代通过jointTask状态和内插根据当前时间。
%返回初始配置表演(currentRobotJConfig的机器人“PreservePlot”假的,“框架”，“关闭”);对于I = 1：长度（trajTimes）％ 当前时间TNOW = trajTimes（ⅰ）;%插入模拟关节位置，以获得当前时刻的配置configNow = interp1（tJoint，stateJoint（：，1：numJoints），TNOW）;poseNow = getTransform(机器人,configNow, endEffector);显示（机器人，configNow，“PreservePlot”假的,“框架”，“关闭”);plot3（poseNow（1,4），poseNow（2,4），poseNow（3,4），'R'。，“MarkerSize”，20）的DrawNow;结束
你也可以从以下列表中选择一个网站:
