Recently,I write a program to demonstrate the forward kinematics of the SCARA robot.The code showed as below:
Using this function to construct the BigArm
and SmallArm
of SCARA robot.
KeywayGeometry[color_List, coordinate_List, length_, height_, radius_,
rotation_List] := Block[
{KeywayGeometryResult},
KeywayGeometryResult =
{
Rotate[
{color[[1]], Dashed, Thick,
Line[{coordinate - {0, 0, 2}, coordinate + {0, 0, height + 2}}],
Line[{coordinate + {length, 0, -2}, coordinate + {length, 0, height + 2}}],
color[[2]],
Cylinder[{coordinate, coordinate + {0, 0, height}}, radius],
Cuboid[coordinate - {0, radius, 0}, coordinate + {length, radius, height}],
Cylinder[{coordinate + {length, 0, 0}, coordinate + {length, 0, height}}, radius]},
Sequence @@ rotation
]
}
]
Main program
Manipulate[
Block[
{CoodinateSystem, ModelCoodinateSystem, Base, BaseHeight = 14,
BigArm, BigArmLength = 35, BigArmHeight = 4, BigArmRadius = 2,
SmallArm, SmallArmLength = 20, SmallArmHeight = 4, SmallArmRadius = 2,
LiftMechanism, LiftMechanismHeight = 18, Wrist,px, py},
px = BigArmLength Cos[?1] + SmallArmLength Cos[?1 + ?2];
py = BigArmLength Sin[?1] + SmallArmLength Sin[?1 + ?2];
CoodinateSystem =
{
Sequence @@ Flatten@
({#1, Arrowheads[.02], Arrow[Tube[#2]]} &@@@
{
{Lighter@Red, {{0, 0, 0}, {10, 0, 0}}},
{Lighter@Green, {{0, 0, 0}, {0, 10, 0}}},
{Blue, {{0, 0, 0}, {0, 0, 10}}}
}),
Lighter@Red,
Sequence @@
(Text[Style[#1, Bold, 12, FontFamily -> Times, Italic], #2] & @@@
{
{"x", {12, 0, 0}}, {"y", {0, 12, 0}}, {"z", {0, 0, 12}}
})
};
ModelCoodinateSystem =
{
Sequence @@ Flatten@
({#1, Arrowheads[.02], Arrow[Tube[#2]]} &@@@
{
{Lighter@Red, {{0, 0, BaseHeight + BigArmHeight},
{10, 0,BaseHeight + BigArmHeight}}},
{Lighter@Green, {{0, 0, BaseHeight + BigArmHeight},
{0, 10, BaseHeight +BigArmHeight}}},
{Blue, {{0, 0, BaseHeight + BigArmHeight},
{0, 0, 10 + BaseHeight + BigArmHeight}}}
}),
};
Base =
{
RGBColor[1, .5, .5, .6],
Cylinder[{{0, 0, 0}, {0, 0, 2}}, 3],
Cylinder[{{0, 0, 2}, {0, 0, BaseHeight}}, 2]
};
BigArm =
KeywayGeometry[
{Red, Blue}, {0, 0, BaseHeight}, BigArmLength, BigArmHeight, BigArmRadius,
{?1, {0, 0, 1}, {0, 0, 0}}];
SmallArm =
KeywayGeometry[
{Green, Purple},{BigArmLength Cos[?1], BigArmLength Sin[?1],
BaseHeight + BigArmHeight}, SmallArmLength, SmallArmHeight, SmallArmRadius,
{?1 + ?2, {0, 0, 1}, {BigArmLength Cos[?1],
BigArmLength Sin[?1], 0}}];
LiftMechanism =
{
Rotate[
Cylinder[
{{px, py, BaseHeight + BigArmHeight - d3},
{px, py, BaseHeight + LiftMechanismHeight + BigArmHeight - d3}}, 1],
?1 + ?2, {0, 0, 1}, {px, py, 0}]
};
Wrist =
Flatten@({#1, Cylinder[#2, #3]} & @@@
{
{Green, {{px, py, BaseHeight + BigArmHeight - d3}, {px, py,
BaseHeight + BigArmHeight - d3 - .5}}, 2.5},
{Red, {{px + 2 Cos[?1 +?2 - ?4],py + 2 Sin[?1 +?2 - ?4],
BaseHeight + BigArmHeight - d3 - .5},
{px + 2 Cos[?1 + ?2 - ?4], py + 2 Sin[?1 + ?2 - ?4],
BaseHeight + BigArmHeight - d3 - 2}}, .5},
{Orange,
{{px + 2 Cos[?1 + ?2 - ?4 + ?], py + 2 Sin[?1 + ?2 - ?4 + ?],
BaseHeight + BigArmHeight - .5 - d3},
{px + 2 Cos[?1 + ?2 - ?4 + ?], py + 2 Sin[?1 + ?2 -?4 + ?],
BaseHeight + BigArmHeight - 2 - d3}}, .5}
});
Panel[
Graphics3D[
Sequence @@ # & /@
{CoodinateSystem, ModelCoodinateSystem, Base, BigArm, SmallArm, LiftMechanism, Wrist},
BoxRatios -> Automatic, ImageSize -> 550, ViewPoint -> {80, -60, 15},
PlotRange -> {{-60, 60}, {-60, 60}, {0, 45}}],
Style["SCARA Model Demostration", FontFamily -> "SketchFlowPrint",Red, 15]]
],
{{?1, 20 \[Degree], "?1"}, -100 \[Degree], 100 \[Degree]},
{{?2, 50 \[Degree], "?2"}, -150 \[Degree], 150 \[Degree]},
{{d3, 5, "d3"}, 0, 10},
{{?4, 20 \[Degree], "?4"}, -180 \[Degree], 180 \[Degree]},
ControlPlacement -> Bottom,
FrameLabel -> Style[" The Kinematics of SCARA Robot " , FontFamily -> Times, 15,Purple]
]
It generrate the picture as below:
However,I want add a module
that can show the position of the endeffector of SCARA robot.
or help the user to read the coordinate of the endeffector.
I have a method that construct a coordinate system that owns detailed calibration.Unfortunately,I don't know how to write the code