# How to construct a coordinate system that owns detailed calibration in the 3D enviroment?

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],
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@
{
{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@
{
{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

-
would wolfram system modeler be better for this? – Mike Honeychurch Jun 16 '14 at 5:40