%%
%额外笔记
%v=[3,4];
%distance=norm(v)
%函数norm代表求向量的模长
%函数repmat(A,m,n)将矩阵整体A向下复制 m 个，向右复制 n 个
%函数kron(A,B)，Kronecker 张量积
%%
% 加载机器人模型
% robot = loadrobot(‘kinovaGen3’,‘DataFormat’,‘row’,‘Gravity’,[0 0 -9.81]);
% currentRobotJConfig = homeConfiguration(robot)
% 函数homeConfiguration(robot’s name)定义机器臂的初始关节位置,数据形式为T(4X4)
% homeconfiguration 函数是表示模型建立的初始位置，不会人为改变
% 函数randomConfiguation(robot’s name)定义机器臂的初始关节位置,数据形式为T(4X4)
% Use the homeConfiguration or randomConfiguation functions to generate the structure that defines all the joint positions.
% load exampleRobots.mat
%%
%向量和角度转化函数
%%函数getTransform 将坐标转化成矩阵T（4X4）
%transform = getTransform(puma1,randomConfiguration(puma1),‘L2’,‘L6’)
%transform = 4×4
% -0.2232 0.4179 0.8807 0.0212
% -0.8191 0.4094 -0.4018 0.1503
% -0.5284 -0.8111 0.2509 -0.4317
% 0 0 0 1.0000
%在robot toolbox中用transl函数转化位置向量，和trot{x,y,z}(x,y,z)转化位置角度
%上下函数功能相同
%在MATLAB当中 用trvec2tform函数转化位置向量，axang2tform函数转化位置角度
%格式 %trvec2tform([x,y,z]), 正转化函数
%tform2trvec（T）将矩阵T转化成坐标形式 逆转化函数
% axang2tform([x,y,z,theta]):旋转那个轴，对应向量上用1or0 正转化函数
% 例子: axang2tform([1,0,0,pi])——x轴旋转180度
% tform2axang（T） 逆转化函数
% eul2tform（[x,y,z]）转化位置角度 正转化函数
% tform2eul(T) 逆转化函数
%%
%四元数
% 函数 q=tform2quat（T） 解出矩阵T的四元数
% T=quat2tform（[a,b,c,d]） 利用四元数解出矩阵T
%%
%轨迹规划
%%%直线规划 111
% T0 初始位置矩阵4X4
% Tf 末端位置矩阵4X4
% timeInterval 运行时间范围
% trajTimes 时间步数
%[T,vel,acc]=transformtraj(T0,Tf,timeInterval,trajTimes) 直线
%%%
% cubic B-spline
% [T,vel,acc，pp]=bsplinepolytraj(controlPoints,tInterval,tSamples) generates a piecewise cubic B-spline trajectory
% controlPoints=[x…;
% y…;
% z…;]
%%%
%三次多项式cubicpolytraj
% [qxyz,qdxyz,qddxyz,pp]=cubicpolytraj(controlPoints, timeInterval, trajTimes);
%%%
%四次多项式quinticpolytraj
% [qxyz,qdxyz,qddxyz,pp]=quinticpolytraj(wayPoints,timePoints,tSamples)
%%%
% 速度梯形规划trapveltraj
% tSamples——trajTimes时间步数。例如：tSamples=0：0.025：5
% numSamples——Number of samples in output trajectory
% [q,qd,qdd,tSamples,pp]=trapveltraj(wayPoints,numSamples,Name,Value)
% [q,qd,qdd,pp]=trapveltraj(wayPoints,numSamples,Name,Value)
%%%
% 角度规划rottraj
% r0=[rx,ry,rz];初始角度位置
% rF=[rx,ry,rz];末端角度位置
% [R,omega,alpha] = rottraj(r0,rF,tInterval,tSamples)
%%
%逆解部分
%part-1 Create an inverse kinematics object 创建逆解器
% ik = inverseKinematics(‘RigidBodyTree’, robotname);
%part-2 调用逆解器进行求解
% QSol = ik(endEffectorName, tform, weights, initialGuess)
% QSol 是机器臂的各个关节角度
% endEffectorName 末端关节名称
% tform 终点位置的Translform
% initialGuess 初始位置Translform。注：可以用函数homeconfiguration 例如：initialGuess=homeconfiguration
%%
% 显示机器臂动画编码
% 由matlab官网例子 \“trajectory-planning-robot-manipulators-master”\
% 思路就是在每次时间步数上
% 对运用轨迹规划的函数 将求得的位置坐标每一个都转化成Translfrom(4X4)
% 将求解的 \T矩阵\ 作为?本次的末端位置? 和作为?下次循环的初始位置?
% 求解对应时间点上 \T矩阵\ 的关节角度（运用逆解“inverseKinematics”“ik”）
% 同时显示机器臂在对应时间上的关节角度 注：show(机器臂名字，joint_angles,‘Frames’,‘off’,‘PreservePlot’,false)
% Joint_angles 就是逆解器 ik 求得的各个关节角度
% 官方例子
for idx = 1:numel(trajTimes)
% Solve IK
tgtPose = trvec2tform(q(:,idx)’); % q 是靠之前轨迹函数所求的坐标\% idx 是轨迹规划的时间步数
[config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess);
ikInitGuess = config; % 将求解的 \T矩阵\ 作为?本次的末端位置? 和作为?下次循环的初始位置?
% Show the robot
show(gen3,config,'Frames','off','PreservePlot',false);
title(['Trajectory at t = ' num2str(trajTimes(idx))])
drawnowend
