从机器人库中生成机器人逆运动学计算代码

打开生活的脚本

这个例子展示了如何使用机器人库中的机器人执行代码生成来计算逆运动学(IK)。在本例中，可以使用aninverseKinematics包含rigidBodyTree机器人模型使用loadrobot求解机器人构型，以达到期望的末端执行器位置。

在二维平面上创建一个圆形轨迹，并将其作为点赋给生成的MEX逆运动学求解器。求解器计算所需的关节位置，以实现这一轨迹。最后，通过动画显示实现圆形轨迹的机器人构型。

编写求解逆运动学的算法

创建一个函数,ikCodegen，它运行KINOVA®Gen3机器人模型的逆运动学算法loadrobot．

函数qConfig = ikCodegen (endEffectorName tform,重量、initialGuess)% # codegen机器人= loadrobot (“kinovaGen3”，“DataFormat”，“行”）;本土知识= inverseKinematics (“RigidBodyTree”,机器人);本土知识[qConfig ~] = (endEffectorName tform,重量、initialGuess);结束

该算法充当标准逆运动学调用的包装器。它接受标准输入，并返回一个机器人配置解决方案向量。因为您不能使用句柄对象作为代码生成支持的函数的输入或输出。金宝app将机器人装载到函数中。保存ikCodegen函数在当前文件夹中。

在MATLAB中验证逆运动学算法

在生成代码之前，在MATLAB中验证IK算法。

加载预先定义的KINOVA®Gen3机器人模型作为rigidBodyTree对象。设置数据格式为“行”．

机器人= loadrobot (“kinovaGen3”，“DataFormat”，“行”）;

显示机器人的详细信息。

showdetails(机器人)

-------------------- 机器人:(8 Body) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) ------------ ---------- ---------- ---------------- ---------------- 1 Shoulder_Link Actuator1 revolute base_link(0) HalfArm1_Link(2) 2 HalfArm1_Link Actuator2 revolute Shoulder_Link(1) HalfArm2_Link(3) 3 HalfArm2_Link Actuator3 revolute HalfArm1_Link(2)腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器腕链执行器--------------------

指定末端执行器名称、末端执行器转换的权重和初始关节位置。

endEffectorName =“EndEffector_Link”；权重= [0.25 0.25 0.25 1 1 1];initialGuess = [0 0 0 0 0];

为指定的末端执行器变换调用逆运动学求解函数。

targetPose = trvec2tform([0.35 -0.35 0]);qConfig = ikCodegen (endEffectorName targetPose,重量、initialGuess)

qConfig =1×71.3085 2.2000 -1.3011 1.0072 -1.1144 2.0500 -3.2313

可视化机器人与计算机器人配置解决方案。

图;显示(机器人,qConfig);持有所有plotTransforms (tform2trvec (targetPose) tform2quat (targetPose),“FrameSize”, 0.5);

图中包含一个轴对象。axis对象包含29个patch、line类型的对象。这些对象代表base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link，腕1_link，腕2_link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh，腕1_link_mesh，腕2_link_mesh, Bracelet_Link_mesh, base_link_mesh。

逆运动学算法的生成代码

你可以用codegen(MATLAB编码器)函数或MATLAB编码器(MATLAB编码器)应用程序生成代码。对于本例，通过调用codegen在MATLAB命令行。属性为函数的每个输入指定示例输入参数arg游戏输入参数。

调用codegen函数并指定单元格数组中的输入参数。这个函数创建了一个单独的ikCodegen_mex函数使用。您还可以使用选项输入参数生成C代码。这一步可能需要一些时间。

codegenikCodegenarg游戏{endEffectorName targetPose,重量,initialGuess}

代码生成成功。

使用生成的MEX函数验证结果

为指定的转换调用IK解算器的MEX版本。

targetPose = trvec2tform([0.35 -0.35 0]);qConfig = ikCodegen_mex (endEffectorName targetPose,重量、initialGuess)

qConfig =1×71.3084 2.2000 -1.2999 1.0092 2.0277 -2.0500 -0.0872

使用IK求解器的MEX版本计算的机器人配置可视化机器人。

图;显示(机器人,qConfig);持有所有plotTransforms (tform2trvec (targetPose) tform2quat (targetPose),“FrameSize”, 0.5);

利用MEX函数计算逆运动学

利用生成的MEX函数计算逆运动学解，实现轨迹。

定义轨迹

创造一个圆形轨迹。

t =(0:0.2:10)”;%的时间数=长度(t);中心= [0.3 0.3 0];半径= 0.15;θ= t *(2 *π/ t(结束));点=中心+半径*[cos(theta) sin(theta) zeros(size(theta))];

逆运动学解

预分配配置解决方案作为一个矩阵金宝搏官方网站qs．指定末端执行器转换的权重和末端执行器名称。

Q0 = [0 0 0 0 0];ndof =长度(q0);q = 0(计数,ndof);Weights = [0 0 0 1 1 1];endEffector =“EndEffector_Link”；

绕着点的轨迹走一圈。使用ikCodegen_mex函数来计算每个点的解，以生成实现末端执行器位置的关节配置。存储配置以供以后使用。

qInitial = q0处;%使用home配置作为初始猜测为i = 1:计数求满足要求的末端执行器的构型%的位置点=点(我:);qSol = ikCodegen_mex (endEffector trvec2tform(点)、重量、qInitial);%存储配置qs(我:)= qSol;%从之前的解决方案开始qInitial = qSol;结束

动画解决方案

一旦你产生了所有的解决方案，动画的结果。金宝搏官方网站您必须重新创建机器人，因为它最初是在函数内部定义的。迭代所有的解决方案。金宝搏官方网站设置“FastUpdate”选择的显示方法真正的得到一个平滑的动画。

机器人= loadrobot (“kinovaGen3”，“DataFormat”，“行”）;%显示第一个解决方案和设置视图。图显示(机器人,qs (1:));视图(3)ax = gca;斧子。投影=“拼写”；持有在情节(点(:1),点(:,2),“k”)轴([-0.1 0.7 -0.3 0.5])迭代解决方案金宝搏官方网站framesPerSecond = 15;r = rateControl (framesPerSecond);为I = 1:count show(机器人，qs(I，:))，“PreservePlot”假的,“FastUpdate”,真正的);drawnow waitfor (r);结束

图中包含一个轴对象。axis对象包含26个类型为patch, line的对象。这个对象表示base_link。