MATLAB Answers

Elliptical trajectory and plot waypoints for robotic arm simulation

59 views (last 30 days)
RANJITH NAIK
RANJITH NAIK on 5 Apr 2021
Edited: Matt J on 5 Apr 2021
How to plot an ellipse on plot3..and also waypoints along the path
I have have a image of circular path.
In same manner how can we plot an ellipse?
final application is simulate a manipulator in in elliptical path.
% LOAD robot into and create rigid tree
% Specify EndEffector joint position
gen3 = loadrobot("kinovaGen3");
gen3.DataFormat = 'column';
q_home = [0 15 180 -130 0 55 90]'*pi/180;
eeName = 'EndEffector_Link';
T_home = getTransform(gen3, q_home, eeName);
% Visualize the robot at home configuration
show(gen3,q_home);
axis auto;
view([60,10]);
% Create Inverse Kinematics Solver and Set Parameters
ik = inverseKinematics('RigidBodyTree', gen3);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1, 1, 1, 1, 1, 1];
q_init = q_home;
%Define Waypoints from the Desired Trajectory
center = [0.5 0 0.4]; %[x y z]
radius = 0.1;
% Define total time and time step, and based on that generate waypoints for the circular trajectory
dt = 0.25;
t = (0:dt:10)';
theta = t*(2*pi/t(end))-(pi/2);
points = center + radius*[0*ones(size(theta)) cos(theta) sin(theta)];
%Plot the waypoints along with the robot at home configuration
hold on;
plot3(points(:,1),points(:,2),points(:,3),'-*g', 'LineWidth', 1.5);
xlabel('x');
ylabel('y');
zlabel('z');
axis auto;
view([60,10]);
grid('minor');
%Solve the Inverse Kinematics for Each Waypoint
numJoints = size(q_home,1);
numWaypoints = size(points,1);
qs = zeros(numWaypoints,numJoints);
for i = 1:numWaypoints
T_des = T_home;
T_des(1:3,4) = points(i,:)';
[q_sol, q_info] = ik(eeName, T_des, weights, q_init);
% Display status of ik result
%disp(q_info.Status);
% Store the configuration
qs(i,:) = q_sol(1:numJoints);
% Start from prior solution
q_init = q_sol;
end
%Visualize the Animation of the Solution
figure; set(gcf,'Visible','on');
ax = show(gen3,qs(1,:)');
ax.CameraPositionMode='auto';
hold on;
% Plot waypoints
plot3(points(:,1),points(:,2),points(:,3),'b','LineWidth',2);
axis auto;
view([60,10]);
grid('minor');
hold on;
title('Simulated Elliptical Movement of the Robot');
% Animate
framesPerSecond = 30;
r = robotics.Rate(framesPerSecond);
for i = 1:numWaypoints
show(gen3, qs(i,:)','PreservePlot',false);
drawnow;
waitfor(r);
end

Accepted Answer

Matt J
Matt J on 5 Apr 2021
Edited: Matt J on 5 Apr 2021
Fairly easy to do with this FEX submission,
gtobj=ellipticalFit.groundtruth([], [0,0],[2,1],90); %Define the ellipe in 2D
xy=gtobj.sample(0:20:360); %Define waypoint coordinates
axis([0 0.6 -0.3 0.3 0 0.6]); grid on
view([-41,-33])
hEllipse=plot(gtobj,{'Color','g'}); %Make plot in xy plane first
hold on
hWayPts=scatter(xy{:},'filled','k','SizeData',50);
hold off
xlabel 'X', ylabel 'Y', zlabel 'Z'
T=makehgtform('translate',[0.4,0,0.4],'yrotate',pi/2); %rototranslate the plot out of xy plane
set([hEllipse,hWayPts],'Parent',hgtransform('Matrix',T))
  7 Comments
Matt J
Matt J on 5 Apr 2021
Looks like you didn't download the files. Or, you didn't put them somewhere on the path where Matlab can seee them.

Sign in to comment.

More Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!