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

1.前记：
【Robotics|Robotics system toolbox中的运动函数与动力学函数（自己学习记录）】动力学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