使用KINOVA Gen3机械手规划和执行任务和关节空间轨迹
这个例子展示了如何生成和模拟从初始到期望的末端执行器姿态移动的内插关节轨迹。轨迹的定时是基于臂端工具(EOAT)的近似期望速度。
加载KINOVA Gen3刚体树(RBT)机器人模型。
机器人= loadrobot (“kinovaGen3”，“DataFormat”，“行”，“重力”[0 0 -9.81]);
设置当前机器人关节配置。
currentRobotJConfig = homeConfiguration(机器人);
得到关节数量和末端执行器RBT框架。
numJoints =元素个数(currentRobotJConfig);endEffector =“EndEffector_Link”；
指定轨迹时间步长和近似期望的刀具速度。
步伐= 0.1;%秒toolSpeed = 0.1;% m / s
设定初始和最终的末端执行器姿态。
jointInit = currentRobotJConfig;taskInit = getTransform(机器人,jointInit, endEffector);taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);
生成任务空间轨迹
通过插值计算任务空间轨迹点。
首先，计算工具的移动距离。
距离=规范(tform2trvec (taskInit) -tform2trvec (taskFinal));
接下来，根据移动距离和所需的工具速度定义轨迹时间。
initTime = 0;finalTime =(距离/工具速度)- initTime;trajTimes = initTime:步伐:finalTime;timeInterval = [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求解器，如数值解决同样的问题要花更长的时间。欲了解更多信息，请参阅选择一个ODE求解器和解决刚性常微分方程在文档中。
由于参考状态在每个时刻都在变化，因此需要一个包装函数将插值的轨迹输入更新为状态导数。因此，将一个示例helper函数作为函数句柄传递给ODE求解器。由此产生的机械手状态输出到stateTask．
[task,stateTask] = ode15s(@(t,state) examplehelpertimebasedtaskputs (tsMotionModel,timeInterval,taskInit,taskFinal,t,state)，timeInterval，[q0;qd0]);
生成关节空间轨迹
为机器人创建一个逆运动学对象。
本土知识= inverseKinematics (“RigidBodyTree”,机器人);ik.SolverParameters.AllowRandomRestart = false;Weights = [1 1 1 1 1];
使用逆运动学计算初始和期望的关节配置。将值包装到pi，以确保插值是在最小的域上。
initialGuess = jointInit;本土知识jointFinal = (endEffector taskFinal,重量、initialGuess);
缺省情况下，IK解决方案尊重关节限制。然而，对于连续关节(具有无限范围的转动关节)，结果值可能会不必要地大，并可能被包裹(π-π,)以确保最后的轨迹覆盖最小的距离。由于Gen3的非连续关节已经在这个区间内有限制，它是足够的简单包装的关节值π．连续关节将被映射到区间(π-π,)，其他值保持不变。
wrappedJointFinal = wrapToPi (jointFinal);
使用三次多项式函数在它们之间进行插值，以生成均匀间隔的关节配置数组。使用b样条生成平滑的轨迹。
ctrlpoints = [jointInit, wrappedJointFinal];jointConfigArray = cubicpolytraj (ctrlpoints timeInterval trajTimes);jointWaypoints = bsplinepolytraj (jointConfigArray timeInterval 1);
控制关节空间轨迹
建立关节空间运动模型，用于关节PD控制。的jointSpaceMotionModel物体模型刚体树模型的运动，并使用比例微分控制在指定的关节位置。
jsMotionModel = jointSpaceMotionModel (“RigidBodyTree”,机器人,“MotionType”，“PDControl”）;
设置初始状态(关节位置和速度)。
q0 = currentRobotJConfig;qd0 = 0(大小(q0));
使用ode15s来模拟机器人的运动。同样，示例helper函数用作ODE求解器的函数句柄输入，以便在每个瞬间及时更新引用输入。在中输出关节空间状态stateJoint．
[tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state)，timeInterval，[q0;qd0]);
可视化和比较机器人的轨迹
显示机器人的初始配置。
表演(currentRobotJConfig的机器人“PreservePlot”假的,“帧”，“关闭”）;持有在轴([-1 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”假的,“帧”，“关闭”）;taskSpaceMarker = 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”假的,“帧”，“关闭”）;jointSpaceMarker = plot3 (poseNow(1、4),poseNow(2、4),poseNow(3、4),“r”。，“MarkerSize”, 20);drawnow;结束%添加一个图例和标题传奇([taskSpaceMarker jointSpaceMarker) {在工作空间中定义的，在关节空间中定义的}）;标题(“机械手轨迹”）
你也可以从以下列表中选择一个网站: