精华内容
下载资源
问答
  • matlab轨迹
    2022-04-12 18:49:08

    trajectoty planning 和 path planning

    • path planning多注重于集合点的生成,主要用于机器人的避障算法,生成路径的中间点
    • trajectory planning注重与从一个起始点到目标点的速度、加速度规划。
      trajectory planning主要用于pathing planning之后,将生成的几何点进行拟合,得到每一段的速度和加速度曲线

    三次样条曲线

    使用三次样条曲线的原因是避免速度的突变,因为在实际系统中,速度突变意味着加速度变化是一个脉冲,而实际的电机不可能产生脉冲输入,因此进行三次样条曲线拟合,保证路径三阶可导,二阶连续,避免上述问题。
    matlab提供自带函数解决三次样条曲线的拟合

    clc
    clear
    close all
    t0 = 0;
    tf = 5;
    q_pt = [1 5 6 3 2 7];
    t_pt = linspace(t0,tf,length(q_pt));
    
    t = linspace(t0,tf,10*length(q_pt));
    % 本函数返回的是对应时间t的q,qdot,qddot
    [q,qdot,qddot,pp] = cubicpolytraj(q_pt,t_pt,t);
    
    

    五次样条曲线

    很多论文中使用五次多项式拟合路径点,这是因为三次样条曲线在加速度中仍有突变,使用五次样条曲线保证加速度也没有突变,对电机拟合更有效。
    代码如下

    clc
    clear
    close all
    t0 = 0;
    tf = 5;
    q_pt = [1 5 6 3 2 7];
    t_pt = linspace(t0,tf,length(q_pt));
    
    t = linspace(t0,tf,10*length(q_pt));
    % 本函数返回的是对应时间t的q,qdot,qddot
    [q,qdot,qddot,pp] = quinticpolytraj(q_pt,t_pt,t);
    

    注意事项

    matlab提供的这两个函数中,三次拟合函数保证了每个点的速度为0,五次拟合函数保证每个点的加速度为0,这与我们通常的要求不符合,我们并不一定希望所有中间点速度都是0。但是maitlab似乎没有提供好的解决方案,而reflexxes库有更好的解决方案。

    更多相关内容
  • 双8字无碳小车运动轨迹,工程训练大赛
  • 轨迹规划三次多项式差值在MATLAB中的使用代码
  • 详细说明:运动图像的自动识别、检测、抽取运动图像的运动轨迹-
  • 产生轨迹,可以产生直线轨迹,直线加速轨迹,转弯轨迹,得到的物体真实运动的轨迹,并且显示出来。
  • matlab实现的轨迹追踪算法,有GUI界面设计,可以操纵笔记本摄像头实现运动目标的追踪,并且绘制运动轨迹
  • 惯性导航轨迹生成器,用于生成惯性导航轨迹
  • 此资源包括机械臂轨迹规划matlab仿真代码,包括多项式仿真、焊接轨迹等功能仿真,基于6自由度关节机器人,在matlab2012上已验证,可直接建立工程运行。
  • Ekf滤波
  • 移动机器人的滑模轨迹跟踪控制,利用MATLAB进行仿真
  • matlab robotics toolbox画出运动轨迹,画出各个关节角的变量图,得到关节空间规划轨迹,得到控制各个关节的输出力矩
  • 给定威胁源,利用蚁群算法规划出从指定起始点到目标点的满足代价函数最小的轨迹
  • 利用matlab实现运动车辆的自动识别,并且可以提取车辆的轨迹、速度等信息。有详细的应用流程介绍。
  • 完整代码已上传我的资源:【行为识别】基于matlab轨迹法行为识别【含Matlab源码 375期】 点击上面蓝色字体,直接付费下载,即可。获取代码方式2: 付费专栏图像处理(Matlab)备注: 点击上面蓝色字体付费专栏图像...

    一、获取代码方式

    获取代码方式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轨迹跟踪代码带有挠性软管的多个四旋翼飞机:动力学,差动平板和控制 关于 此仓库显示了用于实现“”的Matlab代码。 Matlab类MultipleQuadrotorFlexibleHose.m定义并模拟了系统的广义无坐标动力学。 对于任何...
  • 无碳小车Matlab轨迹仿真及路径图.doc
  • matlab机器人工具箱实现机械臂直线轨迹&圆弧轨迹规划(带sw模型+matlab程序+报告)
  • MATLAB轨迹规划 发给ROS中机器人实现仿真运动 现象如图所示: 0、matlab 与 ROS 通信: https://blog.csdn.net/qq_40569926/article/details/112162871 指定matlab路径:连接三句话 pe = pyenv('Version','D:\...

    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

    展开全文
  • MATLAB 轨迹模拟程序

    热门讨论 2009-12-14 20:50:18
    模拟运动目标的运动轨迹,可以用与模拟数据输入及飞机飞行模拟。
  • 无人车轨迹跟踪控制的MATLAB实现,通过simulink实现的。
  • 完整代码,可直接运行
  • matlab轨迹跟踪代码快速局部跟踪 完全实现以下论文中发布的部分跟踪方法的Matlab代码: J. Neri和P. Depalle,“”,在第21届国际数字音频效果会议(DAFx-18)会议录中,葡萄牙阿威罗,第326-333页,2018年9月。 ...
  • 无人驾车车轨迹跟踪控制,是目前社会研究的重点。本程序代码实现matlab联合carsim进行联合仿真,实现追踪给定轨迹。经测试,在低速状态,追踪效果很理想。
  • 基于MATLAB 轨迹跟踪研究重金属铜对鲢幼鱼自发游泳行为的影响.pdf
  • 基于MATLAB轨迹跟踪研究重金属铜对鲢幼鱼自发游泳行为的影响.pdf
  • 相结合来预测轨迹。” 视觉计算国际研讨会。 斯普林格,湛,2019 年。 现在代码有点乱,我们正在做一些清理工作。 如果您有任何问题/查询,请发送电子邮件至。 此存储库包含 :check_mark: ETH 和 UCY 数据集的处理...
  • MATLAB+位置点到轨迹

    2018-09-26 15:06:44
    由目标位置点获得轨迹段,目标发生交叉时即断开,MATLAB代码
  • 基于MATLAB轨迹跟踪研究重金属铜对鲢幼鱼自发游泳行为的影响
  • 基于MATLAB的KUKA焊接机器人轨迹规划与运动学仿真.pdf

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 12,283
精华内容 4,913
关键字:

matlab轨迹

matlab 订阅