# How to construct a coordinate system that owns detailed calibration ?

Posted 9 years ago
3471 Views
|
|
1 Total Likes
|
 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