Trajectory generation using inverse kinematics

Question

Johann 2023-3-27

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1935734-trajectory-generation-using-inverse-kinematics

回答： Kosei Noda 2023-4-24

I have made a rigid body tree model of the UR10e using the following DH-parameters:

dhparams = [0         pi/2    0.1807  0;
            -0.6127   0       0       0;
            -0.57155  0       0       0;
            0        pi/2    0.17415  0;
            0        -pi/2   0.11985  0;
            0         0       0.11655  0];
robot = rigidBodyTree;

I then build the robot succesfully as explained in the rigidBodyTree documentation and am able to get inverse Kinematics solutions using

[configSoln, solInfo]

However, as I try to generate a trajectory for the end-effector or 'body6' to follow points in a coordinate frame according to the this example the robot is not displayed and the end-effector does not follow a trajectory along the selected waypoints. I made some changes to the example code of course in order to fit my robot.

body1 = rigidBody('body1');
body2 = rigidBody('body2');
body3 = rigidBody('body3');
body4 = rigidBody('body4');
body5 = rigidBody('body5');
body6 = rigidBody('body6');
joint1 = rigidBodyJoint('joint1', 'revolute');
joint2 = rigidBodyJoint('joint2', 'revolute');
joint3 = rigidBodyJoint('joint3', 'revolute');
joint4 = rigidBodyJoint('joint4', 'revolute');
joint5 = rigidBodyJoint('joint5', 'revolute');
joint6 = rigidBodyJoint('joint6', 'revolute');
setFixedTransform(joint1, dhparams(1,:), "dh");
setFixedTransform(joint2, dhparams(2,:), "dh");
setFixedTransform(joint3, dhparams(3,:), "dh");
setFixedTransform(joint4, dhparams(4,:), "dh");
setFixedTransform(joint5, dhparams(5,:), "dh");
setFixedTransform(joint6, dhparams(6,:), "dh");
body1.Joint = joint1;
body2.Joint = joint2;
body3.Joint = joint3;
body4.Joint = joint4;
body5.Joint = joint5;
body6.Joint = joint6;
addBody(robot, body1, 'base')
addBody(robot, body2, 'body1')
addBody(robot, body3, 'body2')
addBody(robot, body4, 'body3')
addBody(robot, body5, 'body4')
addBody(robot, body6, 'body5')
%show(robot);
%axis([-0.5,0.5,-0.5,0.5,-0.5,0.5])
%axis off
randConfig = robot.randomConfiguration;
tform = getTransform(robot, randConfig, 'body6', 'base');
show(robot, randConfig);
%create iK solver for robot
ik = inverseKinematics('RigidBodyTree', robot);
weights = [0.25 0.25 0.25 1 1 1];
initialguess = robot.homeConfiguration;
%find joint configurations for specified end-effector pose incl. weights
%i.e. tolerances
[configSoln, solInfo] = ik('body6', tform, weights, initialguess);
%show(robot, configSoln);
toolPositionHome = [0.455    0.001    0.434];
waypoints = toolPositionHome' + ... 
            [0 0 0 ; -0.1 0.2 0.2 ; -0.2 0 0.1 ; -0.1 -0.2 0.2 ; 0 0 0]';
orientations = [pi/2    0   pi/2;
               (pi/2)+(pi/4)      0   pi/2; 
                pi/2    0   pi;
               (pi/2)-(pi/4)       0    pi/2;
                pi/2    0   pi/2]';
waypointTimes = 0:7:28;
ts = 0.25;
trajTimes = 0:ts:waypointTimes(end);
waypointVels = 0.1 *[ 0  1  0;
                     -1  0  0;
                      0 -1  0;
                      1  0  0;
                      0	 1  0]';
waypointAccels = zeros(size(waypointVels));
waypointAccelTimes = diff(waypointTimes)/4;
ikWeights = [1 1 1 1 1 1];
ikInitGuess = initialguess';
plotMode = 1; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames
show(robot, initialguess','Frames','off','PreservePlot',false);
hold on
if plotMode == 1
    hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-');
end
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2);
axis auto;
view([30 15]);
includeOrientation = true; 
numWaypoints = size(waypoints,2);
numJoints = numel(robot.homeConfiguration);
jointWaypoints = zeros(numJoints,numWaypoints);
for idx = 1:numWaypoints
    if includeOrientation
        tgtPose = trvec2tform(waypoints(:,idx)') * eul2tform(orientations(:,idx)');
    else
        tgtPose = trvec2tform(waypoints(:,idx)') * eul2tform([pi/2 0 pi/2]); %#ok<UNRCH> 
    end
    [config, info] = ik('body6', tgtPose, ikWeights, ikInitGuess);
    jointWaypoints(:,idx) = config.JointPosition;
end
trajType = 'trap';
switch trajType
    case 'trap'
        [q,qd,qdd] = trapveltraj(jointWaypoints,numel(trajTimes), ...
            'AccelTime',repmat(waypointAccelTimes,[numJoints 1]), ... 
            'EndTime',repmat(diff(waypointTimes),[numJoints 1]));
                            
    case 'cubic'
        [q,qd,qdd] = cubicpolytraj(jointWaypoints,waypointTimes,trajTimes, ... 
            'VelocityBoundaryCondition',zeros(numJoints,numWaypoints));
        
    case 'quintic'
        [q,qd,qdd] = quinticpolytraj(jointWaypoints,waypointTimes,trajTimes, ... 
            'VelocityBoundaryCondition',zeros(numJoints,numWaypoints), ...
            'AccelerationBoundaryCondition',zeros(numJoints,numWaypoints));
        
    case 'bspline'
        ctrlpoints = jointWaypoints; % Can adapt this as needed
        [q,qd,qdd] = bsplinepolytraj(ctrlpoints,waypointTimes([1 end]),trajTimes);
        
    otherwise
        error('Invalid trajectory type! Use ''trap'', ''cubic'', ''quintic'', or ''bspline''');
end
for idx = 1:numel(trajTimes)  
 
    for joint_num=1:1:size(config, 2)
        config(joint_num).JointPosition = q(joint_num, idx)';
    end
    
    % Find Cartesian points for visualization
    eeTform = getTransform(robot,config,'body6');
    if plotMode == 1
        eePos = tform2trvec(eeTform);
        set(hTraj,'xdata',[hTraj.XData eePos(1)], ...
                  'ydata',[hTraj.YData eePos(2)], ...
                  'zdata',[hTraj.ZData eePos(3)]);
    elseif plotMode == 2
        plotTransforms(tform2trvec(eeTform),tform2quat(eeTform),'FrameSize',0.05);
    end
 
    % Show the robot
    show(robot,config','Frames','off','PreservePlot',false);
    title(['Trajectory at t = ' num2str(trajTimes(idx))])
    drawnow   
    
end

The output looks quite strange as follows:

I appreciate any and all help!

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Karsh Tharyani 2023-3-30

1
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1935734-trajectory-generation-using-inverse-kinematics#answer_1205264

Hi,

You can't see the robot because the lines which should show the robot have been commented out. Additionally, if you wish to visualize the robot and the cartesian frame origins with each joint configuration, you should have to call "hold on".

Please read about axis holding here: https://www.mathworks.com/help/matlab/ref/hold.html

Hope that helps,

Karsh

2 个评论
显示无隐藏无

Johann 2023-3-31

Thank you very much for your help, do you also know why the trajectory does not follow the waypoints selected?

Karsh Tharyani 2023-4-3

Hi Johann,

I would suggest you reach out to our Technical Support team with the reproducible of the issue. That will enable us to diagnose this in an efficient manner.

Best,

Karsh

请先登录，再进行评论。

Answer 2

Kosei Noda 2023-4-24

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1935734-trajectory-generation-using-inverse-kinematics#answer_1221818

在 MATLAB Online 中打开

The problem here is the calculation of jointWaypoints.

I, and you may expect that 'jointWaypoints' has 5 sets of joint angles for each waypoint,

however, all of its rows consists of same values.

The reason why same values are recorded is operation of struct object.

If you access to struct array like >> var = struct.member, it returns only one value.

One idea to avoid the issue is obtaining joint angles like below.

jointWaypoints(:,idx) = arrayfun(@(x) x.JointPosition, config)';

Hope this helps!

P.S.

If you encounter unexpected behavior, debug mode is very helpful tool to solve the issue by yourself.

https://jp.mathworks.com/help/matlab/matlab_prog/debugging-process-and-features.html

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

Trajectory generation using inverse kinematics

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

采纳的回答

2 个评论
显示无隐藏无

更多回答（1 个）

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

另请参阅

类别

标签

产品

Community Treasure Hunt

Trajectory generation using inverse kinematics

0 个评论 显示 -2更早的评论隐藏 -2更早的评论

采纳的回答

2 个评论 显示 无隐藏 无

更多回答（1 个）

0 个评论 显示 -2更早的评论隐藏 -2更早的评论

另请参阅

类别

标签

产品

Community Treasure Hunt

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

2 个评论
显示无隐藏无

0 个评论
显示 -2更早的评论隐藏 -2更早的评论