使用KINOVA Gen3机械手计划和执行任务和关节空间轨迹
这个例子展示了如何生成和模拟插值关节轨迹,以从初始位置移动到所需的末端执行器姿态。轨迹的时间是基于一个近似的期望结束臂工具(EOAT)速度。
加载KINOVA Gen3刚体树(RBT)机器人模型。
机器人=装载机器人(“kinovaGen3”,“DataFormat”,“行”,“重力”,[0 0 -9.81]);
设置当前机器人关节配置。
currentRobotJConfig = homeConfiguration(robot);
得到关节数和末端执行器RBT框架。
numjoint = nummel (currentRobotJConfig);endEffector =“EndEffector_Link”;
指定轨迹时间步长和大致所需的刀具速度。
timeStep = 0.1;%秒toolSpeed = 0.1;% m / s
设置初始和最终末端执行器姿态。
jointInit = currentRobotJConfig;taskkinit = getTransform(robot,jointInit,endEffector);taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);
生成任务空间轨迹
通过插值计算任务空间轨迹路点。
首先,计算刀具移动距离。
distance = norm(tform2trvec(taskkinit)-tform2trvec(taskFinal));
接下来,根据移动距离和所需的刀具速度定义轨迹时间。
initTime = 0;finalTime =(距离/toolSpeed) - 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(大小(q0));
使用ode15s
来模拟机器人的运动。对于这个问题,闭环系统是刚性的,这意味着在问题的某个地方存在缩放的差异。因此,积分器有时被迫采取非常小的步骤,而非僵硬的ODE求解器,如数值
解决同样的问题要花更长的时间。有关更多信息,请参阅选择一个ODE求解器而且解决生硬的颂歌在文档中。
由于参考状态在每个时刻都在变化,因此需要包装器函数在每个时刻将插值轨迹输入更新为状态导数。因此,一个示例帮助函数作为函数句柄传递给ODE求解器。由此产生的操纵器状态输出stateTask
.
[tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state),timeInterval,[q0;qd0]);
生成关节空间轨迹
为机器人创建逆运动学对象。
ik =逆运动学(“RigidBodyTree”,机器人);ik.SolverParameters.AllowRandomRestart = false;权重= [1 1 1 1 1 1];
利用逆运动学计算初始和期望的关节构型。将值包装为pi以确保插值在最小域上。
initialGuess = jointInit;jointFinal = ik(executor,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(大小(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: num关节),tNow);poseNow = getTransform(robot,configNow, enffector);表演(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, enffector);表演(configNow的机器人“PreservePlot”假的,“帧”,“关闭”);jointspacemark = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),“r”。,“MarkerSize”, 20);drawnow;结束添加图例和标题legend([taskSpaceMarker jointSpaceMarker], {在任务空间中定义,在关节空间中定义});标题(“机械手轨迹”)