主要内容

计划和执行任务——和关节空间轨迹使用KINOVA Gen3操纵者

打开生活的脚本

这个例子展示了如何生成和模拟插值联合轨迹从一个初始移动到所需的末端执行器构成。轨迹的时机是基于arm工具(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π]);

生成任务空间轨迹

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

首先,旅行距离的计算工具。

距离=规范(tform2trvec (taskInit) -tform2trvec (taskFinal));

接下来,定义基于轨迹倍旅行距离和速度所需的工具。

initTime = 0;finalTime =(距离/ toolSpeed) - 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模拟机器人运动。对于这个问题,闭环系统是僵硬的,这意味着有一个比例在不同问题。因此,积分器有时被迫采取极其小的步骤,和non-stiff ODE求解器等数值需要更长的时间来解决同样的问题。有关更多信息,请参考选择一个ODE求解器和解决刚性常微分方程在文档中。

自参考状态改变在每个瞬间,需要一个包装器函数来更新插值轨迹输入状态导数在每一个瞬间。因此,一个helper函数作为函数句柄传递ODE求解器进行求解。合成机械手状态输出stateTask。

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

生成关节空间轨迹

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

本土知识= inverseKinematics (“RigidBodyTree”,机器人);ik.SolverParameters。AllowRandomRestart = false;重量= [1 1 1 1 1 1];

计算所需的初始和联合配置使用逆运动学。包装到π的值以确保插值是最小的域。

initialGuess = jointInit;本土知识jointFinal = (endEffector taskFinal,重量、initialGuess);

默认情况下,本土知识解决方案方面联合限制。然而,对于连续关节(与无限的范围内转动关节),合成值可能过于庞大,可以包装(π-π,)以确保最终的轨迹覆盖最小距离。自非连续的关节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,状态)exampleHelperTimeBasedJointInputs (jsMotionModel、timeInterval jointConfigArray, t,状态),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) {在工作空间中定义的,在关节空间中定义的});标题(“机械手轨迹”)

{“字符串”:“图包含一个坐标轴对象。坐标轴对象与标题机械手轨迹包含150行,类型的对象。这些对象代表在工作空间中定义,base_link Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh,在关节空间中定义。”、“特克斯”:“机械手轨迹”、“乳胶”:[]}