利用运动学逆解和Kinova KORTEX MATLAB API跟踪Kinova Gen3机器人末端执行器的预计算轨迹

这个例子使用了:

机械手机器人系统工具箱支持包万博1manbetx 机械手机器人系统工具箱支持包万博1manbetx

打开实时脚本

这个例子展示了如何用刚体树解决逆运动学问题，并移动Kinova®Gen3 7自由度超轻机器人手臂跟随所需的轨迹。

通过右键单击活动脚本中的任何地方，然后单击“清除所有输出”或导航到VIEW >清除所有输出，清除活动脚本中的所有输出。

所需的产品s manbetx 845

机器人系统工具箱
万博1manbetx

创建机器人模型

为Gen3机器人创建刚体树，设置主构型并计算在主构型下的变换:

Gen3 =负载机器人(“kinovaGen3”）;gen3。DataFormat =“列”；Q_home = [0 15 180 -130 0 55 90]'*pi/180;eeName =“EndEffector_Link”；T_home = getTransform(gen3, q_home, eeName);

可视化机器人在家里的配置

显示(gen3 q_home);轴汽车；视图([60,10]);

创建逆运动学求解器并设置参数

的inverseKinematics系统对象™创建一个逆运动学(IK)求解器，根据指定的刚体树模型计算所需末端执行器姿态的关节配置。

ik =逆运动学(“RigidBodyTree”, gen3);

禁用逆运动学求解器的随机重启。AllowRandomRestart参数表示是否允许随机重启。当算法逼近不满足约束条件的解时，会触发随机重启。使用随机生成的初始猜测。

ik.SolverParameters.AllowRandomRestart = false;

指定位姿公差的重量优先级，作为六元向量。前三个元素对应于所需姿态的方向误差权重。最后三个元素对应于期望姿态在xyz位置的错误权重。

权重= [1,1,1,1,1,1];

由于所期望的轨迹被选择在离家位置附近开始，所以将求解器的初始猜测定义为机器人的离家位置。

Q_init = q_home;

从期望轨迹定义路点

本节帮助您创建末端执行器跟踪的圆形轨迹。在整个运动范围内，保持末端执行器的方向不变。定义圆轨迹的中心和半径。

中心= [0.5 0 0.4];%[x y z]半径= 0.1;

定义总时间和时间步长，并在此基础上生成圆轨迹的路点

Dt = 0.25;T = (0:dt:10)';= t*(2*pi/t(end))-(pi/2);点=中心+半径*[0*ones(size(theta)) cos(theta) sin(theta)];

将路径点与机器人在家配置一起绘制

持有在；plot3(点(:1),点(:,2),点(:,3),“- * g”，“线宽”, 1.5);包含(“x”）;ylabel (“y”）;zlabel (“z”）;轴汽车；视图([60,10]);网格(“小”）;

求解每个路径点的逆运动学

逆运动学求解器找到实现指定末端执行器姿态的关节构型。指定构型的初始猜测和姿态六个组成部分公差的期望权重。逆运动学求解器返回关于其收敛性的信息，建议检查解的状态。

本节帮助您找到由前几节计算的路径点定义的固定端执行器方向和可变位置的关节配置。使用当前解作为下一个航路点的初始猜测，以确保轨迹的平滑和连续。取消注释display命令以查看每次逆运动学迭代的状态。

numjoint = size(q_home,1);numWaypoints = size(points,1);qs = 0 (numWaypoints, num关节);为i = 1:numWaypoints T_des = T_home;T_des(1:3,4) = points(i，:)';[q_sol, q_info] = ik(eeName, T_des, weights, q_init);显示ik结果状态% disp (q_info.Status);存储配置q (i，:) = q_sol(1: num关节);%从先前的溶液开始Q_init = q_sol;结束

可视化解决方案的动画

图;集(gcf,“可见”，“上”）;Ax = show(gen3,qs(1，:)');斧子。CameraPositionMode =“汽车”；持有在；%绘制路径点plot3(点(:1),点(:,2),点(:,3),“g”，“线宽”2);轴汽车；视图([60,10]);网格(“小”）;持有在；标题(“机器人的模拟运动”）;%动画framesPerSecond = 30;r = robotics.Rate(framesPerSecond);为i = 1:numWaypoints show(gen3, qs(i，:)'，“PreservePlot”、假);drawnow;等待(r);结束

将轨迹发送到硬件

机器人的期望运动(假设从缩回位置开始)

如果您想向Kinova Gen3机器人发送命令以跟踪计算轨迹，请按“y”并按键盘上的“Enter”键，或按“Enter”键完成示例。

提示=“你想向硬件发送相同的轨迹吗?”Y /n [n]: '；STR =输入(提示符，“年代”）;如果Isempty (str)“n”；结束如果str = =“n”清晰;返回；结束

命令Kinova Gen3机器人跟踪预计算轨迹

如示例中所述连接Kinova Gen3机器人，使用MATLAB操作手臂， Kinova Gen3机器人的MATLAB API支持多种模式对机器人进行万博1manbetx命令。为了达到所需的关节结构或笛卡尔姿态的高级命令不能用于遵循平滑的轨迹，因为机器人总是在处理下一个命令之前到达目的地。因此，连续命令之间的末端执行器速度达到零，这导致了一个起伏的运动。预先计算的关节轨迹可用于命令机器人跟踪一组路点，以确保平稳运动。

有效的预计算关节轨迹是每个关节在每个路径点上的时间戳、角位置、角速度和角加速度的集合。在计算轨迹时，必须遵守一定的约束条件。有关更多信息，请参见SendPreComputedTrajectory而且预计算关节轨迹。

利用数值微分计算每个路径点的关节速度和加速度

Qs_deg = qs*180/pi;Vel = diff(qs_deg)/dt;Vel (1，:) = 0;Vel (end+1，:) = 0;Acc = diff(vel)/dt;Acc (1，:) = 0;Acc (end+1，:) = 0;

插值关节位置、速度和加速度，以确保两个轨迹点之间的时间步长为0.001秒

时间戳= 0:0.001:t(结束);Qs_deg = interp1(t, Qs_deg,timestamp);Vel = interp1(t, Vel,timestamp);Acc = interp1(t, Acc,timestamp);

连接到机器人

万博1manbetxSimulink.importExternalCTypes ((“kortex_wrapper_data.h”));gen3Kinova = kortex();gen3Kinova。ip_address =“192.168.1.10”；isOk = gen3Kinova.CreateRobotApisWrapper();如果isOk disp (“你和机器人连接上了!”）;其他的错误(“建立有效连接失败!”）;结束

可视化机器人的实际运动

标题(“机器人的实际运动”）;[~，~， actuatorFb， ~] = gen3Kinova.SendRefreshFeedback();显示(gen3, ((actuatorFb.position) *π/ 180)”,“PreservePlot”、假);drawnow;

将机器人送到轨迹起点

注意，SendJointAngles的有效输入范围是0到360度，而计算角度的范围是-180到180度。因此，关节角度必须绕0到360度。

jointmd = wrapTo360(qs_deg(1，:));约束类型= int32(0);速度= 0;持续时间= 0;isOk = gen3Kinova。SendJointAngles(jointmd, constraintType, speed, duration);如果isOk disp (“成功”）;其他的disp (SendJointAngles cmd错误）;返回；结束

检查机器人是否到达起始位置

而1 [isOk，~， actuatorFb， ~] = gen3Kinova.SendRefreshFeedback();显示(gen3, ((actuatorFb.position) *π/ 180)”,“PreservePlot”、假);drawnow;如果isOk如果max(abs(wrapTo360(qs_deg(1，:))-actuatorFb.position)) < 0.1 disp(“起点到了。”）打破；结束其他的错误(“SendRefreshFeedback错误”）结束结束

发送预计算轨迹

isOk = gen3Kinova.SendPreComputedTrajectory(qs_deg. sendprecomputedtrajectory)'， vel.'， acc。'， timestamp, size(timestamp,2));如果isOk disp (“SendPreComputedTrajectory成功”）;其他的disp ('SendPreComputedTrajectory命令错误'）;结束

检查机器人是否到达终点位置

而1 [isOk，~， actuatorFb， ~] = gen3Kinova.SendRefreshFeedback();显示(gen3, ((actuatorFb.position) *π/ 180)”,“PreservePlot”、假);drawnow;如果isOk如果max(abs(wrapTo360(qs_deg(end，:))-actuatorFb.position)) < 0.1 disp(“到达终点了。”）打破；结束其他的错误(“SendRefreshFeedback错误”）结束结束

断开与机器人的连接

使用该命令断开Kinova Gen3机器人的连接。

isOk = gen3Kinova.DestroyRobotApisWrapper();

明确的工作空间

清晰;