Robot Kicker (Warning: Imaginary parts of complex X and/or Y arguments ignored)
1 次查看(过去 30 天)
显示 更早的评论
I am simulating a kick for a robot and figure(3) is causing me some hassle. The warning "Imaginary parts of complex X and/or Y arguments ignored" keeps arising.
Is there a way I can stop this from happening and get the kick simulation working properly?
%%
clear all %Clears the Workspace of the Command Window clf %Clears all the previously opened Figures
format Longg %Best of fixed or floating point format with 15 digits for double and 7 digits for single.
BSplineRes = 5000; %Resolution parameter of the B-Spline Step = 1 : BSplineRes; %Used to access all the elements of the first row of BSplineRes Lambda = Step/BSplineRes; %Creating a variable used for constructing the Bezier Curve
% -------------------------------- % % The Lengths of the Robot's Leg % % -------------------------------- %
L1 = 75; %The length of what would be the robot's femur L2 = 75; %The length of what would be the robot's tibia L3 = 32; %The length of what would be the robot's ankle L4 = 54; %The length of what would be the robot's foot
% -------------------------------- % % Forward Kinematics for Stage 1 % % -------------------------------- %
Theta1S1 = 3*pi/4; %The angle between the pelvis and femur (135 degrees) Theta2S1 = pi/2; %The angle between the femur and the tibia (90 degrees) Theta3S1 = pi/4; %The angle between the ankle and the heel (45 degrees) Theta4S1 = -pi/2; %The angle between the tibia and the toes (90 degrees)
X1S1 = L1 * cos(Theta1S1); %These forward kinematics have been Y1S1 = L1 * sin(Theta1S1); %taken directly from my logbook and X2S1 = X1S1 + L2*cos(Theta1S1 + Theta2S1); %been modified for the first stage Y2S1 = Y1S1 + L2*sin(Theta1S1 + Theta2S1); X3S1 = X2S1 + L3*cos(Theta1S1 + Theta2S1 + Theta3S1); Y3S1 = Y2S1 + L3*sin(Theta1S1 + Theta2S1 + Theta3S1); X4S1 = X3S1 + L4*cos(Theta1S1 + Theta2S1 + Theta3S1 + Theta4S1); Y4S1 = Y3S1 + L4*sin(Theta1S1 + Theta2S1 + Theta3S1 + Theta4S1);
% -------------------------------- % % Forward Kinematics for Stage 2 % % -------------------------------- %
Theta1S2 = pi/2; %The angle between the pelvis and femur (90 degrees) Theta2S2 = pi/4; %The angle between the femur and the tibia (45 degrees) Theta3S2 = 3*pi/16; %The angle between the ankle and the heel (33.75 degrees) Theta4S2 = -pi/2; %The angle between the tibia and the toes (90 degrees)
X1S2 = L1 * cos(Theta1S2); %These forward kinematics have been Y1S2 = L1 * sin(Theta1S2); %taken directly from my logbook and X2S2 = X1S2 + L2*cos(Theta1S2 + Theta2S2); %been modified for the second stage Y2S2 = Y1S2 + L2*sin(Theta1S2 + Theta2S2); X3S2 = X2S2 + L3*cos(Theta1S2 + Theta2S2 + Theta3S2); Y3S2 = Y2S2 + L3*sin(Theta1S2 + Theta2S2 + Theta3S2); X4S2 = X3S2 + L4*cos(Theta1S2 + Theta2S2 + Theta3S2 + Theta4S2); Y4S2 = Y3S2 + L4*sin(Theta1S2 + Theta2S2 + Theta3S2 + Theta4S2);
% -------------------------------- % % Forward Kinematics for Stage 3 % % -------------------------------- %
Theta1S3 = pi/4; %The angle between the pelvis and femur (45 degrees) Theta2S3 = 0; %The angle between the femur and the tibia (0 degrees) Theta3S3 = pi/8; %The angle between the ankle and the heel (22.5 degrees) Theta4S3 = -pi/2; %The angle between the tibia and the toes (90 degrees)
X1S3 = L1 * cos(Theta1S3); %These forward kinematics have been Y1S3 = L1 * sin(Theta1S3); %taken directly from my logbook and X2S3 = X1S3 + L2*cos(Theta1S3 + Theta2S3); %been modified for the final stage Y2S3 = Y1S3 + L2*sin(Theta1S3 + Theta2S3); X3S3 = X2S3 + L3*cos(Theta1S3 + Theta2S3 + Theta3S3); Y3S3 = Y2S3 + L3*sin(Theta1S3 + Theta2S3 + Theta3S3); X4S3 = X3S3 + L4*cos(Theta1S3 + Theta2S3 + Theta3S3 + Theta4S3); Y4S3 = Y3S3 + L4*sin(Theta1S3 + Theta2S3 + Theta3S3 + Theta4S3);
% -------------------------------- % % Plotting the Leg's Swing % % -------------------------------- %
figure(1)
hold all set(gca,'Color',[0.87 0.92 0.98]); %Set the figure's background to light blue title('Phases of the Robot Swing', 'FontSize', 15, 'FontWeight', 'Bold'); %Sets the figure's title xlabel('X-Length (mm)', 'FontSize', 11, 'FontWeight', 'Bold'); %Naming the X-axis ylabel('Y-Length (mm)', 'FontSize', 11, 'FontWeight', 'Bold'); %Naming the Y-axis axis([-200 200 -75 225]); %Setting the maximum and minimum values for both axis
S1Plot1 = (plot(0, 0, 'x')); %Plots an 'x' where the top of the leg is at Stage 1 S1Plot2 = (plot(X1S1, Y1S1, 'x')); %Plots an 'x' for the knee at Stage 1 S1Plot3 = (plot(X2S1, Y2S1, 'x')); %Plots an 'x' for the ankle at Stage 1 S1Plot4 = (plot(X3S1, Y3S1, 'x')); %Plots an 'x' for the heel at Stage 1 S1Plot5 = (plot(X4S1, Y4S1, 'x')); %Plots an 'x' for the toe at Stage 1
set(S1Plot1, 'Color', 'Red', 'LineWidth', 2) %Sets the origin (for all stages) to red set(S1Plot2, 'Color', 'Red', 'LineWidth', 2) %Sets the 'x' on the knee to red set(S1Plot3, 'Color', 'Red', 'LineWidth', 2) %Sets the 'x' on the ankle to red set(S1Plot4, 'Color', 'Red', 'LineWidth', 2) %Sets the 'x' on the heel to red set(S1Plot5, 'Color', 'Red', 'LineWidth', 2) %Sets the 'x' on the toe to red
Line1 = (line([0 X1S1 X2S1 X3S1 X4S1], [0 Y1S1 Y2S1 Y3S1 Y4S1])); %All these points are connected set(Line1, 'Color', 'Black', 'LineWidth', 1.5); %(in black) to form Stage 1
S2Plot1 = (plot(X1S2, Y1S2, 'x')); %Plots an 'x' for the knee at Stage 2 S2Plot2 = (plot(X2S2, Y2S2, 'x')); %Plots an 'x' for the ankle at Stage 2 S2Plot3 = (plot(X3S2, Y3S2, 'x')); %Plots an 'x' for the heel at Stage 2 S2Plot4 = (plot(X4S2, Y4S2, 'x')); %Plots an 'x' for the toe at Stage 2
set(S2Plot1, 'Color', 'Blue', 'LineWidth', 2) %Sets the 'x' on the knee to blue set(S2Plot2, 'Color', 'Blue', 'LineWidth', 2) %Sets the 'x' on the ankle to blue set(S2Plot3, 'Color', 'Blue', 'LineWidth', 2) %Sets the 'x' on the heel to blue set(S2Plot4, 'Color', 'Blue', 'LineWidth', 2) %Sets the 'x' on the toe to blue
Line2 = (line([0 X1S2 X2S2 X3S2 X4S2], [0 Y1S2 Y2S2 Y3S2 Y4S2])); %All these points are connected set(Line2, 'Color', 'Black', 'LineWidth', 1.5); %(in black) to form Stage 2
S3Plot1 = (plot(X1S3, Y1S3, 'x')); %Plots an 'x' for the knee at Stage 3 S3Plot2 = (plot(X2S3, Y2S3, 'x')); %Plots an 'x' for the ankle at Stage 3 S3Plot3 = (plot(X3S3, Y3S3, 'x')); %Plots an 'x' for the heel at Stage 3 S3Plot4 = (plot(X4S3, Y4S3, 'x')); %Plots an 'x' for the toe at Stage 3
set(S3Plot1, 'Color', 'Magenta', 'LineWidth', 2) %Sets the 'x' on the knee to magneta set(S3Plot2, 'Color', 'Magenta', 'LineWidth', 2) %Sets the 'x' on the ankle to magneta set(S3Plot3, 'Color', 'Magenta', 'LineWidth', 2) %Sets the 'x' on the heel to magneta set(S3Plot4, 'Color', 'Magenta', 'LineWidth', 2) %Sets the 'x' on the toe to magneta
Line3 = (line([0 X1S3 X2S3 X3S3 X4S3], [0 Y1S3 Y2S3 Y3S3 Y4S3])); %All these points are connected set(Line3, 'Color', 'Black', 'LineWidth', 1.5); %(in black) to form Stage 3
% -------------------------------- % % Forming the Bezier Curve % % -------------------------------- %
HeelPosS1 = [X3S1 Y3S1 0]; %Co-ordinates of the heel at Stage 1 HeelPosS2 = [X3S2-80 Y3S2+80 0]; %Co-ordinates modified to pass through the heel at Stage 2 HeelPosS3 = [X3S3 Y3S3 0]; %Co-ordinates of the heel at Stage 3 Swing = [1 1 0; -2 2 0; 1 -2 1]; %Coordinates for the Bezier Matrix TrajPoints = ones(4, BSplineRes); %The nodes which indicate the Bezier Curve
%Bezier1 = ([1 Lambda(Order1) Lambda(Order1)^2]); %Bezier2 = (([HeelPosS1' HeelPosS2' HeelPosS3']')'-HeelPosS2');
for Order1 = 1 : BSplineRes
%Bezier = ((Bezier1)*Swing*(Bezier2));
Bezier = ([1 Lambda(Order1) Lambda(Order1)^2]*Swing*[HeelPosS1' HeelPosS2' HeelPosS3']')'-HeelPosS2';
TrajPoints(:, Order1) = [Bezier; 0];
end
Index = zeros(1, BSplineRes); %e Order1 = 2; Order2 = 2; Order3 = 1; MaxEdge = 5; %e %OrderCalc1 = ((TrajPoints(1, Order2)) - (TrajPoints(1, Order1))^2); %e %OrderCalc2 = ((TrajPoints(2, Order2)) - (TrajPoints(2, Order1))^2); %e
while (Order2 < BSplineRes) && (Order3 < BSplineRes) %&& is used for scalar value
while ((abs(Index(Order3)) < MaxEdge) && (Order2 < BSplineRes)) %Abs ensures an absolute value
Order2 = Order2 + 1; %Order2 is then incremented by 1
%Index(Order3) = sqrt(OrderCalc1 + OrderCalc2); %e
Index(Order3) = sqrt((TrajPoints(1, Order2) - TrajPoints(1, Order1))^2 + (TrajPoints(2, Order2) - TrajPoints(2, Order1))^2);
TrajPoints(: ,Order3) = TrajPoints(:, Order1); %e
end
Order3 = Order3 + 1; %e
Order1 = Order2; %e
end
BSplineRes = Order3 - 1; %e Step = Step(1 : BSplineRes); %e InitAngle = 0; %e StepAngle = (((pi)/4.5)/BSplineRes); %e
% -------------------------------- % % Plot the Bezier Curve of the Leg % % -------------------------------- %
for Order1 = 1 : BSplineRes
BezColor1 = (plot(TrajPoints(1, Order1), TrajPoints(2, Order1), 'o')); %This plots several 'o's denoting
set(BezColor1, 'Color', 'Green', 'LineWidth', 1.2); %Creates a smooth looking, green curve
InitAngle = ((pi) - (Order1 - 1)*StepAngle); %the Bezier curve named 'Swing'
TrajPoints(4, Order1) = InitAngle; %e
end
LegendDesc1 = ([S1Plot1, S2Plot1, S3Plot1, BezColor1]); %Creates images for the legend legend(LegendDesc1,'Stage 1','Stage 2','Stage 3','Bezier Swing'); %Names the images of the legend
TrajPoints = TrajPoints(:, 1 : BSplineRes); set(gca,'YDir','reverse') %The figures are inverted in the Y-Direction hold off
% -------------------------------- % % Inverse Kinematics to find Theta % % -------------------------------- %
Delta = (pi/2) + TrajPoints(4, :); %Constant 'Delta' is used for Inverse Kinematic calculations (for Theta3) Xterm = TrajPoints(1, :) - L3*cos(Delta); %Constant 'Xterm' is used for Inverse Kinematic calculations (for Theta3) Yterm = TrajPoints(2, :) - L3*sin(Delta); %Constant 'Yterm' is used for Inverse Kinematic calculations (for Theta3) Rterm = sqrt((Xterm.^2) + (Yterm.^2)); %Constant 'Rterm' is used for Inverse Kinematic calculations (for Theta3) Alpha = atan2(Yterm, Xterm); %Constant 'Alpha' is used for Inverse Kinematic calculations (for Theta1) Gamma = acos(-((L2^2 - Rterm.^2 - L1^2) ./ (2*L1*Rterm))); Beta = acos(-((Rterm.^2 - L1^2 - L2^2) ./ (2*L1*L2)));
Theta1 = (Alpha - Gamma); %Pre-defined mathematical equations Theta2 = (pi - Beta); %for angles of Theta. These have Theta3 = (Delta - Theta1 - Theta2); %previously calculated in logbook
%Constm = (((L2^2) - (Rterm.^2) - (L1^1))/(2*Rterm*L1)); %Constant 'm' derived in logbook %Constk = (((Rterm.^2) - (L1^2) - (L2^2))/(2*L1*L2)); %Constant 'k' derived in logbook
%Theta1 = (acos(Constm) + Alpha); %Pre-defined mathematical equations %Theta2 = (acos(Constk) + pi); %for angles of Theta. These have %Theta3 = Delta - Theta1 - Theta2; %previously calculated in logbook.
% -------------------------------- % % Representing Theta Angles in Deg % % -------------------------------- %
figure(2)
hold all set(gca,'Color',[0.87 0.92 0.98]); %Set the figure's background to light blue title('Angular Conversions (Deg)', 'FontSize', 15, 'FontWeight', 'Bold'); %Sets the figure's title xlabel('X-Length (mm)', 'FontSize', 11, 'FontWeight', 'Bold'); %Naming the X-axis ylabel('Y-Length (mm)', 'FontSize', 11, 'FontWeight', 'Bold'); %Naming the Y-axis axis([0 80 -20 250]); %Minimum and Maximum values have been modified
Theta1Conv = (convang(Theta1, 'rad', 'deg')); %Converts values of Theta1 from radians to degrees Theta2Conv = (convang(Theta2, 'rad', 'deg')); %Converts values of Theta2 from radians to degrees Theta3Conv = (convang(Theta3, 'rad', 'deg')); %Converts values of Theta3 from radians to degrees TrajPointsConv = (convang(TrajPoints(4, :), 'rad', 'deg')); %Converts TrajPoints from radians to degrees
Theta1Ang = (plot(Step, (Theta1Conv), '-')); %Shows the angular conversion of Theta1 in a linear pattern Theta2Ang = (plot(Step, (Theta2Conv), '-')); %Shows the angular conversion of Theta2 in a linear pattern Theta3Ang = (plot(Step, (Theta3Conv), '-')); %Shows the angular conversion of Theta3 in a linear pattern TrajPointsAng = (plot(Step, (TrajPointsConv), '-')); %Shows the angular conversion of TrajPoints in a linear pattern
set(Theta1Ang, 'Color', 'Red', 'LineWidth', 1.2) %Makes the Theta1 line red set(Theta2Ang, 'Color', 'Blue', 'LineWidth', 1.2) %Makes the Theta2 line blue set(Theta3Ang, 'Color', 'Cyan', 'LineWidth', 1.2) %Makes the Theta3 line cyan set(TrajPointsAng, 'Color', 'Green', 'LineWidth', 1.2) %Makes the TrajPoints line green
LegendDesc2 = ([Theta1Ang, Theta2Ang, Theta3Ang, TrajPointsAng]); %Creates images for the legend legend(LegendDesc2,'Theta 1','Theta 2','Theta 3','Bezier Swing'); %Names the images of the legend
% -------------------------------- % % Coordinates of the Robot's Leg % % -------------------------------- %
X1 = L1 * cos(Theta1); %(X1, Y1) are the knee coordinates Y1 = L1 * sin(Theta1); %(X2, Y2) are the ankle coordinates X2 = X1 + L2*cos(Theta1 + Theta2); %(X3, Y3) are the heel coordinates Y2 = Y1 + L2*sin(Theta1 + Theta2); %(X4, Y4) are the toe coordinates X3 = X2 + L3*cos(Theta1 + Theta2 + Theta3); Y3 = Y2 + L3*sin(Theta1 + Theta2 + Theta3); X4 = X3 + L4*cos(Theta1 + Theta2 + Theta3 + Theta4S1); Y4 = Y3 + L4*sin(Theta1 + Theta2 + Theta3 + Theta4S1);
% -------------------------------- % % Stepping through the Kick Motion % % -------------------------------- %
figure(3)
hold all set(gca,'Color',[0.87 0.92 0.98]); %Set the figure's background to light blue title('More Phases of the Swing', 'FontSize', 15, 'FontWeight', 'Bold'); %Sets the figure's title xlabel('X-Length (mm)', 'FontSize', 11, 'FontWeight', 'Bold'); %Naming the X-axis ylabel('Y-Length (mm)', 'FontSize', 11, 'FontWeight', 'Bold'); %Naming the Y-axis axis([-200 200 -75 225]); %Minimum and Maximum values have been modified
State = 1; if State == 1
InitLegMotion1 = (plot(0, 0, 'x')); %e
InitLegMotion2 = (plot(X1(1), Y1(1), 'x')); %e
InitLegMotion3 = (plot(X2(1), Y2(1), 'x')); %e
InitLegMotion4 = (plot(X3(1), Y3(1), 'x')); %e
InitLegMotion5 = (plot(X4(1), Y4(1), 'x')); %e
set(InitLegMotion1, 'Color', 'Red', 'LineWidth', 2) %e
set(InitLegMotion2, 'Color', 'Red', 'LineWidth', 2) %e
set(InitLegMotion3, 'Color', 'Red', 'LineWidth', 2) %e
set(InitLegMotion4, 'Color', 'Red', 'LineWidth', 2) %e
set(InitLegMotion5, 'Color', 'Red', 'LineWidth', 2) %e
Line4 = (line([0 X1(1) X2(1) X3(1) X4(1)],... %e [0 Y1(1) Y2(1) Y3(1) Y4(1)])); %e set(Line4, 'Color', 'Black', 'LineWidth', 1.2); %e
for Order1 = 1 : BSplineRes %e
hold all
MidLegMotion1 = (plot(0, 0, 'x')); %e
MidLegMotion2 = (plot(X1(Order1), Y1(Order1), 'x')); %e
MidLegMotion3 = (plot(X2(Order1), Y2(Order1), 'x')); %e
MidLegMotion4 = (plot(X3(Order1), Y3(Order1), 'x')); %e
MidLegMotion5 = (plot(X4(Order1), Y4(Order1), 'x')); %e
set(MidLegMotion2, 'Color', 'Blue', 'LineWidth', 2) %e
set(MidLegMotion3, 'Color', 'Blue', 'LineWidth', 2) %e
set(MidLegMotion4, 'Color', 'Blue', 'LineWidth', 2) %e
set(MidLegMotion5, 'Color', 'Blue', 'LineWidth', 2) %e
Line5 = (line([0 X1(Order1) X2(Order1) X3(Order1) X4(Order1)],... %e
[0 Y1(Order1) Y2(Order1) Y3(Order1) Y4(Order1)])); %e
set(Line5, 'Color', 'Black', 'LineWidth', 1.2); %e
end
else
hold all
StopLegMotion1 = (plot(0, 0, 'x')); %e
StopLegMotion2 = (plot(X1, Y1, 'x')); %e
StopLegMotion3 = (plot(X2, Y2, 'x')); %e
StopLegMotion4 = (plot(X3, Y3, 'x')); %e
StopLegMotion5 = (plot(X4, Y4, 'x')); %e
set(StopLegMotion2, 'Color', 'Magenta', 'LineWidth', 2) %e
set(StopLegMotion3, 'Color', 'Magenta', 'LineWidth', 2) %e
set(StopLegMotion4, 'Color', 'Magenta', 'LineWidth', 2) %e
set(StopLegMotion5, 'Color', 'Magenta', 'LineWidth', 2) %e
Line6 = (line([0 X1 X2 X3 X4], [0 Y1 Y2 Y3 Y4])); %e
set(Line6, 'Color', 'Magenta', 'LineWidth', 1.2); %e
end
BezColor2 = (plot(TrajPoints(1, :), TrajPoints(2, :), 'o')); %This plots several 'o's denoting the Bezier Swing set(BezColor2, 'Color', 'Green', 'LineWidth', 1.2); %e set(gca, 'YDir', 'reverse') %The figures are inverted in the Y-Direction%The figures are inverted in the Y-Direction
0 个评论
回答(1 个)
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Robotics 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!