一、获取代码方式

获取代码方式1：
完整代码已上传我的资源：【行为识别】基于matlab轨迹法行为识别【含Matlab源码 375期】
点击上面蓝色字体，直接付费下载，即可。

获取代码方式2：
付费专栏图像处理（Matlab）

备注：
点击上面蓝色字体付费专栏图像处理（Matlab），扫描上面二维码，付费299.9元订阅海神之光博客付费专栏，凭支付凭证，私信博主，可免费获得5份本博客上传CSDN资源代码（有效期为订阅日起，三天内有效）；
点击CSDN资源下载链接：5份本博客上传CSDN资源代码

二、部分源代码

%%%%%%%
%%% Main training/testing file
close all
clear all;
clc;

IraniDatasetPath = 'C:\Users\lenovo\Desktop\17351584MACH3D-Rodriguez\MACH3D\actions\bend';
trainAction = 'bend';
threshold = 0.73; % bend: 0.725; pjump: 0.76; runleft: 0.73; jack: 0.77; wave1: 0.78
actions = {'bend', 'jack', 'jump', 'pjump', 'run', 'runleft', 'runright', 'side', 'skip', 'walk', 'wave1', 'wave2'};   %  'runleft', 'runright'
actors = {'daria', 'denis', 'eli', 'ido', 'ira', 'lena', 'lyova', 'moshe', 'shahar'};
trainActorID = [1 2 3 4 6 9]; % [1 2 3 4 6 9];   % [1 2 3 4 6 9];    % i.e. Training will be done on: daria, denis, eli, ido, lena, shahar 
testActorID = [5]; % [1, 7, 8];    %s[5 7 8]               % i.e. Testing of the trained 3D MACH filter will be done on all the actions done by: ira, lyova, and moshe

% programRootPath =  'D:\Mikel\mFiles\Image Processing\MACH3D\';

numActions = length(actions);
numTestActors = length(testActorID);
numTrainActors = length(trainActorID);
volumes = cell(1, numTrainActors);

for v = 1 : numTrainActors
    inFile = sprintf('%s_%0.3d.avi', trainAction, trainActorID(v));
    % inFile = [programRootPath, inFile];
    ifp = aviinfo(inFile);
    volume = zeros(ifp.Height, ifp.Width, ifp.NumFrames, 'uint8');
    for f = 1 : ifp.NumFrames
        frame = aviread(inFile, f);
        rgbImg = frame.cdata;
        grayImg = rgb2gray(rgbImg);
        edgeImg = sobel(grayImg);
        volume(:,:,f) = edgeImg;
    end
    volumes{v} = volume;
end
mach3d = train_otmach3(volumes);
mach_file = sprintf('%s_mach.mat', trainAction);
save (mach_file, 'mach3d')


% Save 3D MACH as a short movie clip

machMovieFile = sprintf('%s_mach.avi', trainAction);
mov = avifile(machMovieFile, 'COMPRESSION', 'None', 'FPS', ifp.FramesPerSecond, 'QUALITY', 100);  % "Indeo5" is better and offer more compression than "Cinepak"
figure(1);
for f = 1 : ifp.NumFrames 
    rgbMACH = cat(3, mach3d(:,:,f), mach3d(:,:,f), mach3d(:,:,f));
    m = im2frame(rgbMACH);
    mov = addframe(mov, m);
    imshow(rgbMACH);
    pause(0.040);
end
disp('3D MACH Filter as a Movie');
disp('=========================');
mov = close(mov)


% Test 3-D MACH filter
% =======================
for a = 1 : numActions
    for v = 1 : numTestActors

        inFileTest = [IraniDatasetPath, sprintf('%s\\%s_%s.avi', actions{a}, actors{testActorID(v)}, actions{a})];
       

        iftp = aviinfo(inFileTest);
        volume = zeros(iftp.Height, iftp.Width, iftp.NumFrames, 'uint8');
        figure(2);
        for f = 1 : iftp.NumFrames
            frame = aviread(inFileTest, f);
            rgbImg = frame.cdata;
            grayImg = rgb2gray(rgbImg);
            edgeImg = sobel(grayImg);
            volume(:,:,f) = edgeImg;
    
        end
        tic
        c = fftnormxcorr3(volume, mach3d);
        disp('');
        disp(sprintf('Test Clip: %s_%s.avi', actors{testActorID(v)}, actions{a}));
        disp('============================')
        [cmax, i, j, k] = max3(c)
        toc

        lineWidth = 4;
        color = [255 255 0];


        outFile = sprintf('Result_TrainedOn_%s_TestedOn_%s_%s.avi', trainAction, actions{a}, actors{testActorID(v)}); 

        mov = avifile(outFile, 'COMPRESSION', 'Indeo5', 'FPS', iftp.FramesPerSecond, 'QUALITY', 100);  % "Indeo5" is better and offer more compression than "Cinepak"

        figure(4)
        for f = 1 : iftp.NumFrames - ifp.NumFrames + 1
            [cmax2, i2, j2] = max2(c(:,:,f));
            frame = aviread(inFileTest, f);
            rgbImg = frame.cdata;
            if cmax2 > threshold
                rgbImg = putColorRectangle(rgbImg, i2, j2, i2+ifp.Height-1, j2+ifp.Width-1, lineWidth, color);
                rgbImg = putColorCrossHair(rgbImg, 'rgb', round(i2+ifp.Height/2), round(j2+ifp.Width/2), 25, 25, 3, color);
            end

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 蔡利梅.MATLAB图像处理——理论、算法与实例分析[M].清华大学出版社，2020.
[2]杨丹,赵海滨,龙哲.MATLAB图像处理实例详解[M].清华大学出版社，2013.
[3]周品.MATLAB图像处理与图形用户界面设计[M].清华大学出版社，2013.
[4]刘成龙.精通MATLAB图像处理[M].清华大学出版社，2015.

MATLAB轨迹规划 发给ROS中机器人实现仿真运动

现象如图所示：

0、matlab 与 ROS 通信：

https://blog.csdn.net/qq_40569926/article/details/112162871

 指定matlab路径：连接三句话
pe = pyenv('Version','D:\python2.7.18\python.exe');%多个python 版本可以用此指定
% 下面四行第一次运行时使用
rosshutdown
setenv('ROS_MASTER_URI','http://192.168.93.131:11311');
% setenv('ROS_IP','202.193.75.81');
%Starting ROS MASTER
rosinit();

当matlab多个版本的时候指定python的版本很重要：

pe = pyenv('Version','D:\python2.7.18\python.exe')
rosinit

1、一个轨迹点的运动：

%% 发送目标消息来移动机器人的手臂
%rosaction list 查看
%等待客户端连接到 ros 操作服务器
[rArm, rGoalMsg] = rosactionclient('/probot_elfin/arm_joint_controller/follow_joint_trajectory');
waitForServer(rArm);
% disp(rGoalMsg.Trajectory)
disp(rGoalMsg)
%ubuntu 通过 rosparam get /probot_elfin/arm_joint_controller/joints
%查看joints的名称
%设置机器人 joints的名称
rGoalMsg.Trajectory.JointNames={'elfin_joint1', ...
                                'elfin_joint2', ...
                                'elfin_joint3', ...
                                'elfin_joint4',...
                                'elfin_joint5',...
                                'elfin_joint6'};
% 通过创建 rostrajectory_msgs/JointTrajectoryPoint
% 消息并指定所有7个关节的位置和速度作为矢量, 在臂关节轨迹中创建设定值。将开始时的时间指定为 ros 持续时间对象。
% Point 1
tjPoint1 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
tjPoint1.Positions = zeros(1,6);%六个关节位置都为0
tjPoint1.Velocities = zeros(1,6);%速度位1
tjPoint1.Accelerations=zeros(1,6);%加速度1
tjPoint1.TimeFromStart = rosduration(1.0);
% Point 2
tjPoint2 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
tjPoint2.Positions = [-1.0 0.2 0.1 -1.2 -1.5 -0.3];%六个关节位置
tjPoint2.Velocities = zeros(1,6);
tjPoint2.Accelerations=zeros(1,6);
tjPoint2.TimeFromStart = rosduration(2.0);
rGoalMsg.Trajectory.Points = [tjPoint1,tjPoint2]; %手臂轨迹点从Point1到Point2
sendGoalAndWait(rArm,rGoalMsg); % 发送消息后右臂移动

2、acro 变换为 urdf 模型：

acro 变换为 urdf 模型 这个在Ubuntu 中进行操作：

rosrun xacro xacro elfin10.urdf.xacro > elfin10.urdf --inorder

3、导入URDF 模型后获取机器人的转态：

elfin3 = importrobot('E:\MATLAB_MIN\ROS_MATLAB\elfin_description\urdf\elfin5.urdf');
% % show(elfin3);
% % showdetails(elfin3);
% %% 获取机器人手臂的状态
% 创建一个订阅者, 从机器人中获取关节状态。
jointSub = rossubscriber('joint_states');
% % % 获取当前的联合状态消息。
jntState = receive(jointSub);
% % 将联合状态从联合状态消息分配到机器人所理解的配置结构的字段。
jntPos =JointMsgToStruct(elfin3,jntState);
% 显示状态

%% 可视化当前机器人配置(从虚拟机得到当前的机器人状态，用Show可视化)
show(elfin3,jntPos);

如图所示：

4、创建对象并定义位姿

%% 创建InverseKinematics对象：
ik = robotics.InverseKinematics('RigidBodyTree', elfin3);
%% 禁用随机重新启动以确保安全解决方案一致：
ik.SolverParameters.AllowRandomRestart = false;
%% 为目标姿势的每个组件上的公差指定权重。 
weights = [1 1 1 1 1 1];
initialGuess = jntPos; % current jnt pos as initial guess
endEffectorName = 'elfin_end_link';% 末端的link
%% 指定与任务相关的末端效应器姿势。
% 没有末端执行器 忽略
% % % % 指定末端执行器的名称。
% % % endEffectorName = 'r_gripper_tool_frame';
% % % % 指定罐的初始 (当前) 姿势和所需的最终姿势。
% % % TCanInitial = trvec2tform([0.7, 0.0, 0.55]);
% % % TCanFinal = trvec2tform([0.6, -0.5, 0.55]);
% % % % 定义抓取时端部效应器和罐之间所需的相对变换。
% % % TGraspToCan = trvec2tform([0,0,0.08])*eul2tform([pi/8,0,-pi]);
%% 按照特定轨迹进行运动
% 建立运动学模型
% elfin=robot('elfin');
% 机器人初始位姿
xoy = [0.1845   0.03631     0.353 ]; %位置
rpy = [0 pi/2 0];  %姿态
home = [0.266 0 0.7015];
start_home = home;
end_home =  xoy;
%定义位姿
shome_T = trvec2tform(start_home);
end_T =   trvec2tform(end_home)*eul2tform(rpy);
Tf=end_T;

5、轨迹规划

%% 执行动作
% 获取当前的联合状态
    jntState = receive(jointSub);
    jntPos = JointMsgToStruct(elfin3,jntState);
    
    % 获取末端的T0
    T0 = getTransform(elfin3,jntPos,endEffectorName);
    %在关键航路点之间进行插值的百分比
    numWaypoints = 10;%插值个数
    t = [0 1];
    [s,sd,sdd,tvec] = trapveltraj(t,numWaypoints,'AccelTime',0.4);%相对缓慢
    TWaypoints = transformtraj(T0, Tf, [0 1], tvec, 'TimeScaling', [s; sd; sdd]); % end-effector pose waypoints
     % joint position waypoints
    jntPosWaypoints = repmat(initialGuess, numWaypoints, 1);  
    
    %使用IK为k = 1：numWaypoints计算每个末端执行器姿势航路点的关节位置
    rArmJointNames = rGoalMsg.Trajectory.JointNames;
    rArmJntPosWaypoints = zeros(numWaypoints, numel(rArmJointNames));
   
    % Calculate joint position for each end-effector pose waypoint using IK
    for k = 1:numWaypoints
    jntPos  = ik(endEffectorName, TWaypoints(:,:,k), weights, initialGuess);
    jntPosWaypoints(k,:)  = jntPos ;
    initialGuess = jntPos ;
    % Extract right arm joint positions from jntPos
        rArmJointPos = zeros(size(rArmJointNames));
       for n = 1:length(rArmJointNames)
            rn = rArmJointNames{n};
            idx = strcmp({jntPos.JointName}, rn);
            rArmJointPos(n) = jntPos(idx).JointPosition;
        end  
       rArmJntPosWaypoints(k,:) = rArmJointPos';
     end
    
    % Time points corresponding to each waypoint
    timePoints = linspace(0,3,numWaypoints); 
    % Estimate joint velocity trajectory numerically
    h = diff(timePoints); h = h(1);
    [~, rArmJntVelWaypoints] = gradient(rArmJntPosWaypoints, h);

6、轨迹发送

 %更新轨迹点
    jntTrajectoryPoints = arrayfun(@(~) rosmessage('trajectory_msgs/JointTrajectoryPoint'), zeros(1,numWaypoints)); 
    %     jntTrajectoryPoints = repmat(rosmessage('trajectory_msgs/JointTrajectoryPoint'),1,numWaypoints);
    A = jntTrajectoryPoints.Positions;
    for m = 1:numWaypoints
           
          jntTrajectoryPoints(m).Positions = rArmJntPosWaypoints(m,:);
          jntTrajectoryPoints(m).Velocities = rArmJntVelWaypoints(m,:);
          jntTrajectoryPoints(m).TimeFromStart = rosduration(timePoints(m));
    end
    
    % Visualize robot motion and end-effector trajectory in MATLAB(R)
    hold on
%     for j = 1:numWaypoints
%         show(elfin3, jntPosWaypoints(j,:),'PreservePlot', false);
%         ShowEndEffectorPos(TWaypoints(:,:,j));
%         drawnow;
%         pause(0.1);
%     end
    % Send the right arm trajectory to the robot
    rGoalMsg.Trajectory.Points = jntTrajectoryPoints;
    disp(rGoalMsg.Trajectory)
    sendGoalAndWait(rArm, rGoalMsg);

8、完整代码：

% clear
clc
% 指定matlab路径：连接三句话
% pe = pyenv('Version','D:\python2.7.18\python.exe');
% % setenv('ROS_MASTER_URI','http://202.193.75.81:11311')
% % rosinit
% % Setting ROS_MASTER_URI
% % 下面四行第一次运行时使用
% rosshutdown
% setenv('ROS_MASTER_URI','http://192.168.93.131:11311');
% % setenv('ROS_IP','202.193.75.81');
% %Starting ROS MASTER
% rosinit();

%% 发送目标消息来移动机器人的手臂
%rosaction list 查看
%等待客户端连接到 ros 操作服务器
[rArm, rGoalMsg] = rosactionclient('/probot_elfin/arm_joint_controller/follow_joint_trajectory');
waitForServer(rArm);
% disp(rGoalMsg.Trajectory)
% disp(rGoalMsg)
%ubuntu 通过 rosparam get /probot_elfin/arm_joint_controller/joints
%查看joints的名称
%设置机器人 joints的名称
rGoalMsg.Trajectory.JointNames={'elfin_joint1', ...
                                'elfin_joint2', ...
                                'elfin_joint3', ...
                                'elfin_joint4',...
                                'elfin_joint5',...
                                'elfin_joint6'};
% % 通过创建 rostrajectory_msgs/JointTrajectoryPoint
% % 消息并指定所有7个关节的位置和速度作为矢量, 在臂关节轨迹中创建设定值。将开始时的时间指定为 ros 持续时间对象。
% % Point 1
% tjPoint1 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
% tjPoint1.Positions = zeros(1,6);%六个关节位置都为0
% tjPoint1.Velocities = zeros(1,6);%速度位1
% tjPoint1.Accelerations=zeros(1,6);%加速度1
% tjPoint1.TimeFromStart = rosduration(1.0);
% % Point 2
% tjPoint2 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
% tjPoint2.Positions = [-1.0 0.2 0.1 -1.2 -1.5 -0.3];%六个关节位置
% tjPoint2.Velocities = zeros(1,6);
% tjPoint2.Accelerations=zeros(1,6);
% tjPoint2.TimeFromStart = rosduration(2.0);
% rGoalMsg.Trajectory.Points = [tjPoint1,tjPoint2]; %手臂轨迹点从Point1到Point2
% sendGoalAndWait(rArm,rGoalMsg); % 发送消息后右臂移动

%% acro 变换为 urdf 模型
% rosrun xacro xacro elfin10.urdf.xacro > elfin10.urdf --inorder
%% matlab 导入机器人urdf 模型
% elfin3 = importrobot('E:\MATLAB_MIN\ROS_MATLAB\elfin_description\urdf\elfin5.urdf');
% % show(elfin3);
% % showdetails(elfin3);
% %% 获取机器人手臂的状态
% 创建一个订阅者, 从机器人中获取关节状态。
jointSub = rossubscriber('joint_states');
% % % 获取当前的联合状态消息。
jntState = receive(jointSub);
% % 将联合状态从联合状态消息分配到机器人所理解的配置结构的字段。
jntPos =JointMsgToStruct(elfin3,jntState);
% 显示状态

%% 可视化当前机器人配置(从虚拟机得到当前的机器人状态，用Show可视化)
% show(elfin3,jntPos);
%% 创建robotics.InverseKinematics pr2机器人对象的逆变运动学对象。
% 逆运动学的目的是计算 pr2 臂的关节角度, 将夹持器 (即末端执行器) 置于所需的姿势。
% 在一段时间内, 一系列的末端效应器被称为轨迹。
% 因此, 在规划过程中, 我们对抬起关节设置了严格的限制。
% 没有抬起关节不需要
% torsoJoint = elfin3.getBody('elfin_link4').Joint
% idx = strcmp({jntPos.JointName}, torsoJoint.Name);
% torsoJoint.HomePosition = jntPos(idx).JointPosition;
% torsoJoint.PositionLimits = jntPos(idx).JointPosition + [-1e-3,1e-3];
%% 创建InverseKinematics对象：
ik = robotics.InverseKinematics('RigidBodyTree', elfin3);
%% 禁用随机重新启动以确保安全解决方案一致：
ik.SolverParameters.AllowRandomRestart = false;
%% 为目标姿势的每个组件上的公差指定权重。 
weights = [1 1 1 1 1 1];
initialGuess = jntPos; % current jnt pos as initial guess
endEffectorName = 'elfin_end_link';% 末端的link
%% 指定与任务相关的末端效应器姿势。
% 没有末端执行器 忽略
% % % % 指定末端执行器的名称。
% % % endEffectorName = 'r_gripper_tool_frame';
% % % % 指定罐的初始 (当前) 姿势和所需的最终姿势。
% % % TCanInitial = trvec2tform([0.7, 0.0, 0.55]);
% % % TCanFinal = trvec2tform([0.6, -0.5, 0.55]);
% % % % 定义抓取时端部效应器和罐之间所需的相对变换。
% % % TGraspToCan = trvec2tform([0,0,0.08])*eul2tform([pi/8,0,-pi]);
%% 按照特定轨迹进行运动
% 建立运动学模型
% elfin=robot('elfin');
% 机器人初始位姿
xoy = [0.1845   0.03631     0.353 ]; %位置
rpy = [0 pi/2 0];  %姿态
home = [0.266 0 0.7015];
start_home = home;
end_home =  xoy;
%定义位姿
shome_T = trvec2tform(start_home);
end_T =   trvec2tform(end_home)*eul2tform(rpy);
Tf=end_T;
%% 执行动作
% 获取当前的联合状态
    jntState = receive(jointSub);
    jntPos = JointMsgToStruct(elfin3,jntState);
    
    % 获取末端的T0
    T0 = getTransform(elfin3,jntPos,endEffectorName);
    %在关键航路点之间进行插值的百分比
    numWaypoints = 10;%插值个数
    t = [0 1];
    [s,sd,sdd,tvec] = trapveltraj(t,numWaypoints,'AccelTime',0.4);%相对缓慢
    TWaypoints = transformtraj(T0, Tf, [0 1], tvec, 'TimeScaling', [s; sd; sdd]); % end-effector pose waypoints
     % joint position waypoints
    jntPosWaypoints = repmat(initialGuess, numWaypoints, 1);  
    
    %使用IK为k = 1：numWaypoints计算每个末端执行器姿势航路点的关节位置
    rArmJointNames = rGoalMsg.Trajectory.JointNames;
    rArmJntPosWaypoints = zeros(numWaypoints, numel(rArmJointNames));
   
    % Calculate joint position for each end-effector pose waypoint using IK
    for k = 1:numWaypoints
    jntPos  = ik(endEffectorName, TWaypoints(:,:,k), weights, initialGuess);
    jntPosWaypoints(k,:)  = jntPos ;
    initialGuess = jntPos ;
    % Extract right arm joint positions from jntPos
        rArmJointPos = zeros(size(rArmJointNames));
       for n = 1:length(rArmJointNames)
            rn = rArmJointNames{n};
            idx = strcmp({jntPos.JointName}, rn);
            rArmJointPos(n) = jntPos(idx).JointPosition;
        end  
       rArmJntPosWaypoints(k,:) = rArmJointPos';
     end
    
    % Time points corresponding to each waypoint
    timePoints = linspace(0,3,numWaypoints); 
    % Estimate joint velocity trajectory numerically
    h = diff(timePoints); h = h(1);
    [~, rArmJntVelWaypoints] = gradient(rArmJntPosWaypoints, h);
    %更新轨迹点
    jntTrajectoryPoints = arrayfun(@(~) rosmessage('trajectory_msgs/JointTrajectoryPoint'), zeros(1,numWaypoints)); 
    %     jntTrajectoryPoints = repmat(rosmessage('trajectory_msgs/JointTrajectoryPoint'),1,numWaypoints);
    A = jntTrajectoryPoints.Positions;
    for m = 1:numWaypoints
           
          jntTrajectoryPoints(m).Positions = rArmJntPosWaypoints(m,:);
          jntTrajectoryPoints(m).Velocities = rArmJntVelWaypoints(m,:);
          jntTrajectoryPoints(m).TimeFromStart = rosduration(timePoints(m));
    end
    
    % Visualize robot motion and end-effector trajectory in MATLAB(R)
    hold on
%     for j = 1:numWaypoints
%         show(elfin3, jntPosWaypoints(j,:),'PreservePlot', false);
%         ShowEndEffectorPos(TWaypoints(:,:,j));
%         drawnow;
%         pause(0.1);
%     end
    % Send the right arm trajectory to the robot
    rGoalMsg.Trajectory.Points = jntTrajectoryPoints;
    disp(rGoalMsg.Trajectory)
    sendGoalAndWait(rArm, rGoalMsg);                         
                               

9、几个函数

ShowEndEffectorPos

function ShowEndEffectorPos( T )
%EXAMPLEHELPERSHOWENDEFFECTORPOS Plot end-effector position

% Copyright 2016 The MathWorks, Inc.

s = 0.005;
[X0,Y0,Z0]=sphere;

X = s*X0 + T(1,4);
Y = s*Y0 + T(2,4);
Z = s*Z0 + T(3,4);
surf(X, Y, Z, 'facecolor','r', 'linestyle','none');

end

JointMsgToStruct

function jntPos = JointMsgToStruct(robot,jntState)
%   exampleHelperJointMsgToStruct The order of body names in the received 
%   jntState message is different from that the pr2 model in MATLAB would
%   expect. This function is to provide a convenient conversion.

% Copyright 2016 The MathWorks, Inc.

jntPos = robot.homeConfiguration;
% 根据jntState.Name的实际个数改正
for i = 1:length(jntState.Name)-1
    idx = strcmp({jntPos.JointName}, jntState.Name{i});
    jntPos(idx).JointPosition = jntState.Position(i);
end

for i = 1:robot.NumBodies
    joint = robot.Bodies{i}.Joint;
    if ~strcmp(joint.Type,'fixed')
        idx = strcmp({jntPos.JointName}, joint.Name);
        jntPos(idx).JointPosition = max(min(jntPos(idx).JointPosition, joint.PositionLimits(2)), joint.PositionLimits(1));
    end
end
end

例子：
https://download.csdn.net/download/qq_40569926/18233484

10、博客内容主要来源 mathwork 官网：

https://www.mathworks.com/help/robotics/ug/control-pr2-arm-movements-using-actions-and-ik.html#PR2ManipulationExample-5

https://blog.csdn.net/weixin_39090239/article/details/84378770