Robotics System Toolbox学习笔记（三）：jointSpaceMotionModel、taskSpaceMotionModel

本文软件版本

Matlab：R2019b Robotics System Toolbox

jointSpaceMotionModel

给定关节空间的输入，对刚体树模型的运动进行建模。  

% 创建默认的二自由度操作臂的运动模型
motionModel = jointSpaceMotionModel

% 创建指定rigidBodyTree对象的运动模型
motionModel = jointSpaceMotionModel("RididBodyTree", tree)

% 设置指定为名称-值对的其他属性,可以按任何顺序指定多个属性
motionModel = jointSpaceMotionModel(Name, Value)

  性质：  
 

derivative

机械手模型状态的时间导数。  

% taskMotionModel---taskSpaceMotionModel类的对象，定义了运动模型的性质

% refPose---4*4齐次变换矩阵，表示末端执行器在任务空间中的参考位姿，以米为单位

% refVel---6*1向量，表示末端执行器在任务空间中的参考速度，指定为[omega v],omega表示围绕x，y和z轴的三个角速度的行向量，单位为rad/s，v表示沿x，y和z轴的三个线速度的行向量，以米每秒为单位

% fExt---外力，指定为m元素向量，其中m是关联的rigidBodyTree对象中的刚体数量

% state---关节位置和速度，2n元素向量，指定为[q; qDot]，n是motionModel关联的rigidbodyBodyTree对象中的关节数，q表示每个关节的位置，以弧度表示。qDot表示每个关节的速度，以rad/s为单位

% cmds---指示所需运动的控制命令，n*2矩阵或者n*3矩阵
% cmds的大小取决于运动模型的MotionType属性。对于"PDControl"，cmds为n*2矩阵，即[qRef; qRefDot]，分别表示关节位置和速度；对于"ComputedTorqueControl"和"IndependentJointMotion"，cmds是一个n*3矩阵，[qRef; qRefDot; qRefDDot]，分别表示关节位置、速度、加速度

% 使用任务空间模型基于当前状态和运动命令来计算运动模型的时间导数
stateDot = derivative(taskMotionModel,state,refPose,refVel)
stateDot = derivative(taskMotionModel,state,refPose, refVel,fExt)
stateDot = derivative(jointMotionModel,state,cmds)
stateDot = derivative(jointMotionModel,state,cmds,fExt)

% stateDot---基于当前状态和指定控制命令的时间导数，返回的是[qDot; qDDot]

updateErrorDynamicsFromStep

给定所需的阶跃响应，更新NaturalFrequency和DampingRatio属性的值。  

% motionModel---jointSpaceMotionModel对象
% settlingTime---过渡时间Ts，直到被控参数进入新的稳态值2%公差带(s)所需的时间，指定为标量或n元素向量。n为机器人活动关节
% overshoot---阶跃系统超调，指定为标量或n元素向量，n为机器人活动关节
% jointIndex---在给定单位步长动态误差的情况下，更新了NaturalFrequency和DampingRatio的关节标号。在这种情况下，必须将过渡时间和超调量指定为标量

% 给定所需的阶跃响应，更新给定的jointSpaceMotionModel对象motionModel的NaturalFrequency和DampingRatio属性的值。
updateErrorDynamicsFromStep(motionModel,settlingTime,overshoot)

% 更新特定关节的NaturalFrequency和DampingRatio属性。在这种情况下，必须将SettlingTime和Overshoot的值作为标量提供，因为它们适用于单个关节
updateErrorDynamicsFromStep(motionModel,settlingTime,overshoot,jointIndex)

例子

% 创建机器人
robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);

% 设置仿真时间0到1s，步长为0.01s
% 设置机器人的初始状态：home构型+零初始速度
tspan = 0:0.01:1;
initialState = [homeConfiguration(robot); zeros(7,1)];

% 定义目标状态的位置、速度和加速度。速度和加速度均为0
targetState = [pi/4; pi/3; pi/2; -pi/3; pi/4; -pi/4; 3*pi/4; zeros(7,1); zeros(7,1)];

% 创建运动模型Motion Model
% 通过ComputedTorqueControl对系统robot进行建模，动态误差由具有5%超调的适度快速阶跃响应定义
motionModel = jointSpaceMotionModel("RigidBodyTree",robot);
updateErrorDynamicsFromStep(motionModel,.3,.05);

% 使用模型的derivative函数作为ode45求解器的输入，以此来模拟1s内的行为
[t,robotState] = ode45(@(t,state)derivative(motionModel,state,targetState),tspan,initialState);

% 画出响应
% 绘制所有到达其目标状态的关节位置。
% 起始位置和目标位置之间位移较大的关节比位移较小的关节以更快的速度到达目标。
% 这会导致出现超调，但是所有关节的调节时间都相同
figure
plot(t,robotState(:,1:motionModel.NumJoints));
hold all;

plot(t,targetState(1:motionModel.NumJoints)*ones(1,length(t)),"--");
title("Joint Position (Solid) vs Reference (Dashed)");
xlabel("Time (s)")
ylabel("Position (rad)");

    输出为：  
 

taskSpaceMotionModel

给定任务空间的参考输入，对刚体树模型的运动进行建模。  

% 与上文中写道的jointSpaceMotionModel对应的函数类似，只是这里都是在任务空间下进行的
motionModel = taskSpaceMotionModel
motionModel = taskSpaceMotionModel("RigidBodyTree",tree)
motionModel = taskSpaceMotionControlModel(Name,Value)

  性质与jointSpaceMotionModel也类似，不同点在于加了末端执行器及关节阻尼系数特性：  
  同样具有derivative和updateErrorDynamicsFromStep函数，具体见上文。

例子

% 创建任务空间运动模型
% 本示例说明如何为任务空间中的机械手机械臂创建和使用taskSpaceMotionModel对象

% 创建机器人模型
robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81])

% 设置仿真参数
% 设置仿真时间0到1s，步长为0.02s
% 设置机器人的初始状态：home构型+零初始速度
tspan = 0:0.02:1;
initialState = [homeConfiguration(robot);zeros(7,1)];

% 定义一个参考目标状态，包括目标位置和零初始速度
refPose = trvec2tform([0.6 -.1 0.5]); % 此时旋转矩阵为I，位置向量变为[0.6 -0.1 0.5]
refVel = zeros(6,1);

% 创建运动模型
% 在比例微分（PD）控制下进行系统建模
% 指定robot刚体树模型，指定末端执行器模型
motionModel = taskSpaceMotionModel("RigidBodyTree",robot,"EndEffectorName","EndEffector_Link");

% 开始仿真
% 使用刚性求解器在1秒钟内进行仿真，以更有效地捕获机器人动力学
% 使用ode15可以在变化率较高的区域周围实现更高的精度
[t,robotState] = ode15s(@(t,state)derivative(motionModel,state,refPose,refVel),tspan,initialState);

% 画出响应
% 画出机器人的位置，并将目标位置用符号"X"标记出来
figure
show(robot,initialState(1:7));
hold all
plot3(refPose(1,4),refPose(2,4),refPose(3,4),"x","MarkerSize",20)

% 通过将机器人绘制成5Hz循环来观察响应
r = rateControl(5);
for i = 1:size(robotState,1)
    show(robot,robotState(i,1:7)',"PreservePlot",false);
    
    % 画出末端轨迹
    poseNow = getTransform(robot, robotState(i, 1: 7)', "EndEffector_Link");
    plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'b.','MarkerSize',20)
    waitfor(r);
end

show

这个函数前面已经用到过很多次了，在figure中显示机器人模型。  

% 使用预定义的home configuration在图中绘制机器人模型的body frame。框架和视觉效果都将自动显示
show(robot)

% 指定机器人robot的构型
show(robot,configuration)

% 提供由一个或多个“名称/值”对参数指定的其他选项
show(___,Name,Value)

% 返回绘制机器人的坐标轴
ax = show(___)

    此处的名Name-Value参数对为：  

• ‘Parent’——轴的父级，指定为以逗号分隔的一对，由父级和在其中绘制机器人的轴对象组成。默认情况下，将在活动轴上绘制机器人。
• ‘PreservePlot’——保留机械手绘图的选项，指定为逗号分隔的对，由“ PreservePlot”和true或false组成，其值默认为true。如果将此属性设置为true，则不会覆盖通过调用show显示的先前图。此设置的功能类似于为标准MATLAB图形调用hold on，但仅限于机器人主体框架。当此属性设置为false时，机器人的先前图将被覆盖。
• ‘Frame’——刚体坐标轴展示，指定为”on”或者”off”，默认为”on”，展示机器人模型树上各个刚体的坐标系。
• ‘Visual’——可视化参数设置，指定为”on”或者”off”，默认为”on”。

例子：使用KINOVA Gen3机械手完成任务空间和关节空间的轨迹

% 加载KINOVA Gen3机器人模型树
% 定义重力加速度为[0 0 -9.81]
robot = loadrobot('kinovaGen3','DataFormat','row','Gravity',[0 0 -9.81]);

% 得到目前robot的关节构型，home构型
currentRobotJConfig = homeConfiguration(robot);

% 获取关节数和末端执行器RBT框架
numJoints = numel(currentRobotJConfig);
endEffector = "EndEffector_Link";

% 指定轨迹时间步长以及期望的末端轨迹速度
timeStep = 0.1; % 秒
toolSpeed = 0.1; % m/s

% 设置末端执行器的初始位姿与最终位姿
jointInit = currentRobotJConfig;
taskInit = getTransform(robot, jointInit, endEffector);
% axang2tform角轴旋转
% 以轴角形式给出的旋转，指定为n个轴角旋转的n×4矩阵。
% 每行的前三个元素指定旋转轴，最后一个元素定义旋转角度（以弧度为单位）
taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);

% 执行任务空间轨迹
% 通过插值计算任务空间轨迹的waypoints
% 首先，计算末端执行器从初始位姿到最终位姿的距离
distance = norm(tform2trvec(taskInit)-tform2trvec(taskFinal));

% 然后，根据预先得到的运动距离及期望的末端轨迹速度计算得到轨迹时间
initTime = 0;
finalTime = (distance/toolSpeed) - initTime;
trajTimes = initTime: timeStep: finalTime;
timeInterval = [trajTimes(1); trajTimes(end)];

% 在taskInit和taskFinal之间进行插值以计算中间任务空间的插值点
[taskWaypoints,taskVelocities] = transformtraj(taskInit,taskFinal,timeInterval,trajTimes);

% 控制任务空间运动
% 为关节上的PD控制创建任务空间运动模型
% taskSpaceMotionModel对象在任务空间PD控制下对刚体树模型的运动进行建模
tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','EndEffector_Link');

% 将orientation方向(向量的前三个元素)上的比例和微分增益设置为零，以便受控行为仅遵循参考位置
tsMotionModel.Kp(1:3,1:3) = 0;
tsMotionModel.Kd(1:3,1:3) = 0;

% 定义初始状态，即关节的位置以及速度
q0 = currentRobotJConfig; 
qd0 = zeros(size(q0));

% 开始仿真
% 使用ode15s模拟机器人运动
% 由于参考状态在每个时刻都发生变化，因此需要封装函数将输入的插值轨迹更新为每个时刻的状态导数
% 这里的exampleHelperTimeBasedTaskInputs函数是内部函数，该函数将时变输入传递给taskSpaceMotionModel对象的derivative函数作为输入
% stateDot = exampleHelperTimeBasedTaskInputs(motionModel, timeInterval, initialTform, finalTform, t, state)
% 在未来可能被移除，是下面两个函数的封装
% [refPose, refVel] = transformtraj(initialTform, finalTform, timeInterval, t);
% stateDot = derivative(motionModel, state, refPose, refVel);
% 因此，示例辅助函数将作为函数句柄传递给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];

% 利用逆运动学计算得到初始与所需要的关节构型
initialGuess = wrapToPi(jointInit);
jointFinal = ik(endEffector,taskFinal,weights,initialGuess);
jointFinal = wrapToPi(jointFinal);

% 使用三次多项式函数在它们之间进行插值以生成均匀间隔的关节角度数组
% 使用B样条曲线生成平滑轨迹
ctrlpoints = [jointInit',jointFinal'];
jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes);
jointWaypoints = bsplinepolytraj(jointConfigArray,timeInterval,1);

% 控制关节空间轨迹
% 为关节上的PD控制创建关节空间运动模型
% jointSpaceMotionModel对象为刚体树模型的运动建模，并在指定的关节位置上使用PD控制
jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');

% 定义初始状态，即关节的位置以及速度
q0 = currentRobotJConfig; 
qd0 = zeros(size(q0));

% 使用ode15s模拟机器人运动
% 同样，示例辅助函数用作ODE求解器的函数句柄输入，以便在每个时间点更新参考输入
% stateDot = exampleHelperTimeBasedJointInputs(motionModel, timeInterval, configWaypoints, t, state)
% 是下面两个函数的封装体，该函数将时变输入传递给jointSpaceMotionModel对象的derivative函数作为输入
% [qd, qdDot] = bsplinepolytraj(configWaypoints,  timeInterval , t);
% stateDot = derivative(motionModel, state, [qd; qdDot]);
% 关节空间状态在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');
    plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'b.','MarkerSize',20)
    drawnow;
end

% 可视化关节空间的轨迹。遍历stateJoint状态并根据当前时间进行插值
% 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');
    plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'r.','MarkerSize',20)
    drawnow;
end

function stateDot = exampleHelperTimeBasedJointInputs(motionModel, timeInterval, configWaypoints, t, state)
    % Use a B-spline curve to ensure the trajectory is smooth and moves
    % through the waypoints with non-zero velocity
    [qd, qdDot] = bsplinepolytraj(configWaypoints,  timeInterval , t);
    
    % Compute state derivative
    stateDot = derivative(motionModel, state, [qd; qdDot]);
end

function stateDot = exampleHelperTimeBasedTaskInputs(motionModel, timeInterval, initialTform, finalTform, t, state)

[refPose, refVel] = transformtraj(initialTform, finalTform, timeInterval, t);

stateDot = derivative(motionModel, state, refPose, refVel);

end

来源

Robot Models — Functions Plan and Execute Task- and Joint-space Trajectories using KINOVA Gen3 Manipulator