标签：timeInterval trajTimes 轨迹 笛卡尔 robot 算法 poseNow 空间 关节
本实例为如何生成和模拟插值关节轨迹，从一个初始运动到一个理想的末端执行器姿态。
轨迹的定时是基于手臂工具(EOAT)的一个近似的期望末端速度。
加载KINOVA Gen3刚体树(RBT)机器人模型
robot = loadrobot('kinovaGen3','DataFormat','row','Gravity',[0 0 -9.81]);设置当前机器人关节位形
currentRobotJConfig = homeConfiguration(robot);得到关节个数和末端执行器RBT框架
numJoints = numel(currentRobotJConfig);
endEffector = "EndEffector_Link";指定轨迹时间步长和所需的末端执行器速度
timeStep = 0.1; % seconds
toolSpeed = 0.1; % m/s设置末端执行器的初始和最终姿态
jointInit = currentRobotJConfig;
taskInit = getTransform(robot,jointInit,endEffector);
taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);生成笛卡尔空间轨迹
通过插值计算任务空间轨迹点
首先，计算末端移动距离：
distance = norm(tform2trvec(taskInit)-tform2trvec(taskFinal));接下来，根据移动距离和所需的末端速度定义轨迹时间
initTime = 0;
finalTime = (distance/toolSpeed) - initTime;
trajTimes = initTime:timeStep:finalTime;
timeInterval = [trajTimes(1); trajTimes(end)];在taskkinit和taskFinal之间插值，计算中间笛卡尔空间的轨迹点
[taskWaypoints,taskVelocities] = transformtraj(taskInit,taskFinal,timeInterval,trajTimes);控制笛卡尔空间运动
建立关节空间运动模型，对关节进行PD控制。taskSpaceMotionModel对象对刚体树模型在任务空间比例微分控制下的运动进行建模。
tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','EndEffector_Link');将方向上的比例增益和导数增益设置为零，这样受控行为就会遵循参考位置:
tsMotionModel.Kp(1:3,1:3) = 0;
tsMotionModel.Kd(1:3,1:3) = 0;定义初始状态(关节位置和速度)
q0 = currentRobotJConfig;
qd0 = zeros(size(q0));使用ode15s来模拟机器人的运动。
对于这个问题，闭环系统是刚性的，这意味着在这个问题的某个地方存在缩放上的差异。
因此，积分器有时被迫采取非常小的步骤，而像ode45这样的非刚性ODE求解器将需要更长的时间来解决相同的问题。
由于参考状态在每一时刻都会发生变化，因此需要一个包装器函数来将插值后的轨迹输入更新为每一时刻的状态导数。
因此，一个示例helper函数作为函数句柄传递给ODE求解器。最终机械手状态输出到stateTask中。
[tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state),timeInterval,[q0; qd0]);生成关节空间轨迹
为机器人创建一个逆运动学对象
ik = inverseKinematics('RigidBodyTree',robot);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1 1 1 1 1 1];使用逆运动学计算初始和期望的关节构型。将值涵盖为pi，以确保插值在最小域上。
initialGuess = jointInit;
jointFinal = ik(endEffector,taskFinal,weights,initialGuess);默认情况下，IK解决方案尊重关节的限制。但对于连续关节(无限范围的转动关节)，其结果值可能会大到不必要的程度，可以包含到[-pi, pi]，以确保最终轨迹覆盖最小距离。由于Gen3的非连续关节在这个区间内已经有限制，因此只需将关节值封装到pi即可。连续关节将被映射到区间[-pi, pi]，其他值保持不变。
wrappedJointFinal = wrapToPi(jointFinal);在它们之间使用三次多项式函数插值来生成均匀间隔关节配置的数组。
使用b样条曲线生成平滑的轨迹。
ctrlpoints = [jointInit',wrappedJointFinal'];
jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes);
jointWaypoints = bsplinepolytraj(jointConfigArray,timeInterval,1);关节空间轨迹控制
建立关节空间运动模型，对关节进行PD控制。jointSpaceMotionModel对象对刚体树模型的运动进行建模，并对指定的关节位置使用比例-导数控制。
jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');设置初始状态(关节位置和速度)
q0 = currentRobotJConfig;
qd0 = zeros(size(q0));使用ode15s来模拟机器人的运动。
同样，我们使用一个示例帮助函数作为函数句柄来处理ODE求解器的输入，以便在每个时刻更新引用输入。
关节空间状态以stateJoint的形式输出。
[tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state),timeInterval,[q0; qd0]);可视化和机器人轨迹对比
显示机器人的初始配置
show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off');
hold on
axis([-1 1 -1 1 -0.1 1.5]);可视化任务空间轨迹。
遍历stateTask状态并根据当前时间插值。
for i=1:length(trajTimes)
% Current time
tNow= trajTimes(i);
% Interpolate simulated joint positions to get configuration at current time
configNow = interp1(tTask,stateTask(:,1:numJoints),tNow);
poseNow = getTransform(robot,configNow,endEffector);
show(robot,configNow,'PreservePlot',false,'Frames','off');
taskSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'b.','MarkerSize',20);
drawnow;
end可视化关节空间轨迹。
迭代jointTask状态并基于当前时间进行插值。
% Return to initial configuration
show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off');
for i=1:length(trajTimes)
% Current time
tNow= trajTimes(i);
% Interpolate simulated joint positions to get configuration at current time
configNow = interp1(tJoint,stateJoint(:,1:numJoints),tNow);
poseNow = getTransform(robot,configNow,endEffector);
show(robot,configNow,'PreservePlot',false,'Frames','off');
jointSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'r.','MarkerSize',20);
drawnow;
end
% Add a legend and title
legend([taskSpaceMarker jointSpaceMarker], {'Defined in Task-Space', 'Defined in Joint-Space'});
title('Manipulator Trajectories')
标签：timeInterval,trajTimes,轨迹,笛卡尔,robot,算法,poseNow,空间,关节 来源： https://blog.csdn.net/weixin_51367832/article/details/122606344
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。