利用KINOVA Gen3机械手规划和执行任务和关节空间轨迹

打开实时脚本

这个例子展示了如何生成和模拟插值关节轨迹，从初始移动到期望的末端执行器姿态。轨迹的定时是基于近似期望的臂端工具(EOAT)速度。

加载KINOVA Gen3刚体树(RBT)机器人模型。

机器人=装载机器人“kinovaGen3”，“DataFormat”，“行”，“重力”，[0 0 -9.81]);

设置当前机器人关节配置。

currentRobotJConfig = homeConfiguration(机器人);

得到关节数量和末端执行器RBT框架。

numjoint = numl (currentRobotJConfig);endEffector =“EndEffector_Link”；

指定轨迹时间步长和所需的近似刀具速度。

timeStep = 0.1;%秒toolSpeed = 0.1;% m / s

设置初始和最终的末端执行器位姿。

jointInit = currentRobotJConfig;taskkinit = getTransform(robot,jointInit, effffector);taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);

生成任务空间轨迹

通过插值计算任务空间轨迹路径点。

首先，计算刀具移动距离。

距离= norm(tform2trvec(taskInit)-tform2trvec(taskFinal));

接下来，根据移动距离和所需的工具速度定义轨迹时间。

initTime = 0;finalTime =(距离/工具速度)- initTime;trajTimes = initTime:timeStep:finalTime;timeInterval = [trajTimes(1);trajTimes(结束)];

之间插入taskInit而且taskFinal计算中间任务空间路径点。

[taskWaypoints, taskvelocity] = transformtraj(taskkinit,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 (size(q0));

使用ode15s来模拟机器人的运动。对于这个问题，闭环系统是刚性的，这意味着在问题的某个地方有不同的缩放。因此，积分器有时被迫采取非常小的步骤，而非刚性ODE求解器如数值要解决同样的问题需要更长的时间。有关更多信息，请参见选择一个ODE求解器而且解决僵硬的颂歌在文档中。

由于参考状态在每个时刻都在变化，因此需要一个包装器函数在每个时刻将插值轨迹输入更新为状态导数。因此，一个示例助手函数作为函数句柄传递给ODE求解器。合成的机械手状态输出在stateTask．

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

生成关节空间轨迹

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

ik = inverseKinematics(“RigidBodyTree”,机器人);ik.SolverParameters.AllowRandomRestart = false;Weights = [1 1 1 1 1 1];

利用逆运动学计算初始和期望的关节构型。将值包装为pi，以确保插值在最小域上。

initialGuess = jointInit;jointFinal = ik(effffector,taskFinal,weights,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 (size(q0));

使用ode15s来模拟机器人的运动。同样，一个示例助手函数被用作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: numjoins)，tNow);poseNow = getTransform(robot,configNow, effffector);表演(configNow的机器人“PreservePlot”假的,“帧”，“关闭”）;taskspacemark = 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: numjoint)，tNow);poseNow = getTransform(robot,configNow, effffector);表演(configNow的机器人“PreservePlot”假的,“帧”，“关闭”）;jointspacemark = plot3(poseNow(1,4)，poseNow(2,4)，poseNow(3,4)，“r”。，“MarkerSize”, 20);drawnow;结束添加一个图例和标题legend([taskSpaceMarker jointSpaceMarker]， {“在任务空间中定义”，“在关节空间中定义”});标题(“机械手轨迹”）