Trajectory generation using inverse kinematics
조회 수: 12 (최근 30일)
이전 댓글 표시
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:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1337214/image.png)
I appreciate any and all help!
댓글 수: 0
채택된 답변
Karsh Tharyani
2023년 3월 30일
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".
Hope that helps,
Karsh
댓글 수: 2
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
추가 답변 (1개)
Kosei Noda
2023년 4월 24일
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
참고 항목
카테고리
Help Center 및 File Exchange에서 ROS Toolbox에 대해 자세히 알아보기
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!