Elliptical trajectory and plot waypoints for robotic arm simulation

10 次查看（过去 30 天）
RANJITH NAIK
RANJITH NAIK 2021-4-5
编辑： Matt J 2021-4-5
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

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

采纳的回答

Matt J
Matt J 2021-4-5
编辑：Matt J 2021-4-5
Fairly easy to do with this FEX submission,
https://www.mathworks.com/matlabcentral/fileexchange/87584-object-oriented-tools-for-fitting-conics-and-quadrics
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 个评论
显示 5更早的评论
Matt J
Matt J 2021-4-5
编辑：Matt J 2021-4-5
Looks like you didn't download the files. Or, you didn't put them somewhere on the path where Matlab can seee them.

请先登录，再进行评论。

更多回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Robotics System Toolbox 的更多信息

标签

Community Treasure Hunt

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

Start Hunting!

Translated by