Robotics system toolbox中的运动函数与动力学函数（自己学习记录）

1.前记：

动力学Peter的Robotics toolbox中使用牛顿-欧拉推导的，https://blog.csdn.net/weixin_39090239/article/details/90270357中部分介绍。以下为Robotics system toolbox中的运动函数与动力学函数。

2、代码：主要以Baxter为对象做了一些计算实验。

%% 机械手臂算法部分MATLAB coder与simulink block同，以下为细研。主要的函数都在classdef RigidBodyTree类中定义。
% For more details visit：https://www.mathworks.com/help/robotics/manipulators.html?s_tid=CRUX_lftnav
%参考网址中有大多数函数，以下为部分解释例子。
% 1、导入并显示机器人模型,对模型处理部分为Robot Models中的函数，
%包括导入机器人，添加或去除部件，添加或清除可视化数据模型到刚体等如下格式，其他参考链接
% addVisual(body,"Mesh",filename)
% addVisual(body,"Mesh",filename,tform)
%或者用 importrobot('YourRobot.urdf');在图框中出现机器人模型，
%smimport为生成simulink中的多体模型，并在Mechanics Explorers中显示
baxter=importrobot('baxter.urdf');
%%
%show可视化结果在figure中显示,此结果是默认配置的机器人状态
show(baxter);
%% 查看某一个部件的物体属性在对应的URDF文件中也可以查看到，RigidBody属性
%部件属性包括名字，质量，质心，惯性，父级和子级，可视化文件名称{'Mesh:W2.STL'}
rightwrist=baxter.Bodies{37}

%% 给一个随机角度配置，再看一下结果。显然各个角度发生了变化。
q=randomConfiguration(baxter);%随机构型
q1=homeConfiguration(baxter);%Home构型
show(baxter,q)
show(baxter,q1)
%% 查看细节 部件名,关节名 .索引序号

showdetails(baxter)

%% 某个部件（right_gripper）相对机器人底座(base)的 Homogeneous Transformation(相对位置和角度)。
Transf=getTransform(baxter,q,'right_gripper') %齐次变换

%% 动力学--整体的质心
baxter.DataFormat = 'row';
com = centerOfMass(baxter)%机器人质心 [x y z] vector.
com1 = centerOfMass(baxter,q)%某构型下的质心com = centerOfMass(robot,configuration)
com2 = centerOfMass(baxter,q1)
[comLocation,comJac] = centerOfMass(baxter)%质心雅克比矩阵3-by-n matrix
%% 相对于底座构成的外力矩阵
% fext = externalForce(robot,bodyname,wrench)
% fext = externalForce(robot,bodyname,wrench,configuration)
baxter.Gravity = [0 0 -9.81];%定义重力方向
wrench = [0 0 0.5 0 0 0.3];%定义扳手外力，前三个为力矩，后三个为力
fext = externalForce(baxter,'right_gripper',wrench,q1) %showdetails中40--right_gripper--right_endpoint   
fext1= externalForce(baxter,'right_wrist',wrench,q1)%作用在right_wrist上，37--right_wrist--right_w2 
fext2= externalForce(baxter,'right_upper_shoulder',wrench,q1)%right_upper_shoulder序号30

%% forwardDynamics：Joint accelerations given joint torques and states
%格式总：jointAccel = forwardDynamics(robot,configuration,jointVel,jointTorq,fext)
qddot = forwardDynamics(baxter,q1,[],[],fext);%计算由于重力，外力作用于抓手上产生的加速度。[]中数值默认为0

%% geometricJacobian:Geometric Jacobian for robot configuration
%格式：jacobian = geometricJacobian(robot,configuration,endeffectorname)
geoJacob = geometricJacobian(baxter,q1,'right_gripper') %6-by-n matrix，n=15
%雅克比矩阵的作用，将关节空间的速度映射到末端执行器处,公式如下：
%[wx wy wz vx vy vz]'=Jqd=J[qd1 qd2 ...qdn]',w为角速度,v线速度,qd关节空间速度
%% 重力矩--Joint torques that compensate gravity关节力矩补偿重力
%格式：
% gravTorq = gravityTorque(robot)
% gravTorq = gravityTorque(robot,configuration)
gtau = gravityTorque(baxter,q1)
%     关于机器人构型的函数
%% 逆动力学：Required joint torques for given motion
%格式总：
%jointTorq = inverseDynamics(robot,configuration,jointVel,jointAccel,fext)
tau = inverseDynamics(baxter,q1);%Compute Inverse Dynamics from Static Joint Configuration此结果与重力矩相同
%Compute the joint torques required to balance the external forces. To combine the forces,
%add the force matrices together. Joint velocities and accelerations are assumed to be zero (input as []).
tau1 = inverseDynamics(baxter,q1,[],[],fext1+fext2);
%                  关于构型，速度，加速度，外力的函数

%% massMatrix惯量矩阵Joint-space mass matrix---positive-definite symmetric matrix
%格式：H = massMatrix(robot,configuration)
%n-by-n的正定对称矩阵，n为机器人自由度，关于机器人构型的函数
H = massMatrix(baxter,q1);
%% velocityProduct：Joint torques that cancel velocity-induced forces
%科氏向心力？关于构型和速度的函数
%格式：jointTorq = velocityProduct(robot,configuration,jointVel)
%Joint torques, specified as a vector. 
%Each element corresponds to a torque applied to a specific joint
qdot = [0 0 0 0 0.2 0.3 0 0.1 0 0 0 0 0 0 0];%定义速度
tau2 = -velocityProduct(baxter,q1,qdot);
%计算机器人在q1配置时，消除速度引起的关节扭矩所需的力矩值。
%速度引起的关节扭矩等于velocityProduct输出的负值。

后记：后续写对照Peter的验证两者的正确性。另外，simulink模块中也有对应的封装好的计算模块。

参考：https://www.mathworks.com/help/robotics/manipulators.html?s_tid=CRUX_lftnav

            https://www.mathworks.com/help/robotics/ug/robot-dynamics.html