Take the 2-minute tour ×
Mathematica Stack Exchange is a question and answer site for users of Mathematica. It's 100% free, no registration required.

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:

SCARA Model

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

share|improve this question
1  
would wolfram system modeler be better for this? –  Mike Honeychurch Jun 16 at 5:40

Your Answer

 
discard

By posting your answer, you agree to the privacy policy and terms of service.

Browse other questions tagged or ask your own question.