Version 3 of Simplified Robot Arm

Updated 2007-03-04 06:21:53

This is a butcher job on the Moving an anthropomorphous robot arm.

I needed a simpler example for my students--this is a first attempt.


 proc deg2rad { angle_degrees } {expr {($angle_degrees)*0.0174532925199}}
 proc rad2deg { angle_radians } {expr {($angle_radians)*57.2957795131}}

 proc math_sin_cos { angle nickname } {
     uplevel [list set cos_$nickname [expr {cos(($angle))}]]
     uplevel [list set sin_$nickname [expr {sin(($angle))}]]
 }

 proc matrix_rotation_z { angle } {
   math_sin_cos $angle t
   list $cos_t $sin_t 0 0  [expr -$sin_t] $cos_t 0 0 0 0 1 0  0 0 0 1
 }

 proc matrix_rotation_x { angle } {
   math_sin_cos $angle t
   list 1 0 0 0  0 $cos_t $sin_t 0  0 [expr -$sin_t] $cos_t 0 0 0 0 1
 }

 proc matrix_rotation_y { angle } {
   math_sin_cos $angle t
   list $cos_t 0 $sin_t 0  0 1 0 0  [expr -$sin_t] 0 $cos_t 0 0 0 0 1
 }

 proc matrix_translation {x y z} {
   #list 1 0 0 $x 0 1 0 $y 0 0 1 $z 0 0 0 1
   list 1 0 0 0 0 1 0 0 0 0 1 0  $x $y $z 1
 }

 proc matrix_identity {} {
   list 1 0 0 0 0 1 0 0 0 0 1 0  0 0 0 1
 }

 proc matrix_multiply { matA matB } {
   # use tcl trick to assign these variables
   foreach {b11 b12 b13 b14  b21 b22 b23 b24  b31 b32 b33 b34  b41 b42 b43 b44} $matB {}

   set result {}
   foreach {x y z w} $matA {
     lappend result \
       [expr {$x*$b11+$y*$b21+$z*$b31+$w*$b41}] \
       [expr {$x*$b12+$y*$b22+$z*$b32+$w*$b42}] \
       [expr {$x*$b13+$y*$b23+$z*$b33+$w*$b43}] \
       [expr {$x*$b14+$y*$b24+$z*$b34+$w*$b44}] 
   }
   return $result
 }

 proc apply_current_matrix { coords } {
   set ::currentmatrix [matrix_multiply $::worldmatrix $::viewmatrix]

   # use tcl trick to assign these variables
   foreach {a11 a12 a13 a14  a21 a22 a23 a24  a31 a32 a33 a34  a41 a42 a43 a44} $::currentmatrix {}

   set result {}
   foreach {x y z ONE} $coords {
     lappend result \
       [expr {$x*$a11+$y*$a21+$z*$a31+ $a41}] \
       [expr {$x*$a12+$y*$a22+$z*$a32+ $a42}] \
       [expr {$x*$a13+$y*$a23+$z*$a33+ $a43}] \
       1
   }
   set coords $result

   set result {}
   foreach { x y z o } $coords {
     if { $::perspective == 0 } {
       lappend result [expr $x + $::screencenter_x] \
                      [expr $::screencenter_y - $y]
     } else {
       set z [expr $z + 1500]
       if { $z == 0 } { set z 1 }
       lappend result [expr $x/$z*1200 + $::screencenter_x] \
                      [expr $::screencenter_y - $y/$z*1200]
     }

   }
   return $result
 }

 ## ------------------------------------------------------------
 ## Basic graphical elements.
 ## ------------------------------------------------------------

 proc draw_3D_line { xyzcoords {obj_tag {}} {tags {}} } {
   set coords [apply_current_matrix $xyzcoords]

   if { ![info exists ::id($obj_tag)] } {
     set ::id($obj_tag) [$::maincanvas create line $coords -tags \
                               [lappend tags $obj_tag] ]
   } else {
     $::maincanvas coords $::id($obj_tag) $coords
   }

 }

 proc draw_prism { prism_tag xsize ysize zsize \
                             xoffset yoffset zoffset {tags Robot} } {
   foreach {face coords} {
     A {-1 -1 -1  1    1 -1 -1  1    1  1 -1  1   -1  1 -1  1   -1 -1 -1  1}
     B {-1 -1  1  1    1 -1  1  1    1  1  1  1   -1  1  1  1   -1 -1  1  1}
     C {-1 -1 -1  1   -1 -1  1  1   -1  1  1  1   -1  1 -1  1   -1 -1 -1  1}
     D { 1 -1 -1  1    1 -1  1  1    1  1  1  1    1  1 -1  1    1 -1 -1  1}
     } {
       set results {}
       foreach {x y z one} $coords {
         lappend results \
           [expr $x * $xsize + $xoffset] \
           [expr $y * $ysize + $yoffset] \
           [expr $z * $zsize + $zoffset] \
           1
       }
       set coords $results
       draw_3D_line $coords ${prism_tag}_$face $tags
     }
 }

 proc draw_plane { obj_tag indices {tags {}} } {
      set delta 30.0
      set num 11
      set max [expr {$delta*($num-1)}]
      set coords { 0 0 0 1  0 0 0 1}
      set counter 0
      foreach { a b c } $indices {
        set vector $coords
        set plane_tag ${obj_tag}_[incr counter]
        for {set x 0.0} {$x <= $max} {set x [expr {$x+$delta}]} {
          lset vector $a $x
          lset vector $b $x
          lset vector $c $max
          draw_3D_line $vector ${plane_tag}_$x [concat [list $obj_tag] $tags]
        }
      }
 }

 ## ------------------------------------------------------------
 ## TK options.
 ## ------------------------------------------------------------

 option add *command_buttons.exit.text        "Exit"

 ## ------------------------------------------------------------
 ## Widget procedures.
 ## ------------------------------------------------------------

 proc create_scale { linkname master varname {from 0} {to 90} {default 0}} {
   set ::mem($varname.from) $from
   set ::mem($varname.to) $to
   set ::mem($varname.default) $default
   set $varname $default

   set f [labelframe $master.$varname -class [string totitle $varname]]
   set column_index 0

   label [set label_widget $f.lab_$varname] -text $linkname
   scale [set scale_widget $f.$varname] \
     -from $::mem($varname.from) -to $::mem($varname.to)

   $scale_widget set $::mem($varname.default)

   $scale_widget configure -command \
       [list update_parameter_from_scale_widget $varname]

   grid $label_widget -column $column_index -row 0 -sticky news
   grid $scale_widget -column $column_index -row 1 -sticky news
   incr column_index

   return $f
 }

 proc update_parameter_from_scale_widget { param_name param_value } {
   set $param_name $param_value
   after 0 update_and_draw
 }

 ## ------------------------------------------------------------
 ## Main procedures.
 ## ------------------------------------------------------------

 proc main_exit {} {exit}

 proc main_program {} {

      widget_setup_defaults

      # configure_toplevel
      wm geometry . +20+20
      wm title . "Simple 3D Robot Arm"

      grid columnconfigure . 0 -weight 1
      grid rowconfigure    . 0 -weight 1

      set ::maincanvas [canvas .drawing -width 600 -height 600 -bg #f8f8f8]
      frame .controlsFrame
      grid  $::maincanvas .controlsFrame  -sticky news

      set camerabox [build_camera_controls .controlsFrame]
      set robotbox  [build_robot_controls  .controlsFrame]
      grid  $camerabox -sticky news
      grid  $robotbox  -sticky news

      set ::bp [button .controlsFrame.pers -command toggle_perspective]
      $::bp config -text "Perspective is On"
      grid $::bp
      grid [button .controlsFrame.exit -command main_exit -text "Exit"]

      update_and_draw

      configure_drawing ;# do this once
 }

 proc toggle_perspective {} {
   if {$::perspective} {
     set ::perspective 0
      $::bp config -text "Perspective is Off"
   } else {
     set ::perspective 1
      $::bp config -text "Perspective is On"
   }
   after 0 update_and_draw
 }

 proc set_worldmatrix {mat} {
     set ::worldmatrix $mat
 }
 proc set_viewmatrix {mat} {
     set ::viewmatrix  $mat
 }

 proc mult_worldmatrix {mat} {
     set ::worldmatrix [matrix_multiply $mat $::worldmatrix]
 }
 proc mult_viewmatrix {mat} {
     set ::viewmatrix [matrix_multiply $mat $::viewmatrix]
 }

 proc get_worldmatrix {} {
     return $::worldmatrix
 }
 proc get_viewmatrix {} {
     return $::viewmatrix
 }

 #============================================================
 proc update_and_draw {} {

    set_viewmatrix  [matrix_rotation_x [deg2rad $::cam_angle1]]
   mult_viewmatrix  [matrix_rotation_y [deg2rad $::cam_angle2]]

    set_worldmatrix [matrix_identity]

   # draw workspace -----------------------------------
   draw_plane WorkSpace_XY  { 0 4 5   1 5 4 } [list WorkSpace]
   draw_plane WorkSpace_YZ  { 1 5 6   2 6 5 } [list WorkSpace]
   draw_plane WorkSpace_ZX  { 2 6 0   4 0 6 } [list WorkSpace]
   draw_3D_line {0 0 0 1   300   0   0 1} XAXIS
   draw_3D_line {0 0 0 1     0 300   0 1} YAXIS
   draw_3D_line {0 0 0 1     0   0 300 1} ZAXIS

   ### move this
   set ::mem(link0) [matrix_rotation_y [deg2rad $::theta_base]]
   set ::mem(link1) [matrix_rotation_x [deg2rad $::anngle1]]
   set ::mem(link2) [matrix_rotation_x [deg2rad $::anngle2]]
   set ::mem(link3) [matrix_rotation_x [deg2rad $::anngle3]]
   set ::mem(link4) [matrix_rotation_z [deg2rad $::theta_hand]]

   set ::mem(left5)  [matrix_translation        $::sllde5  0 0]
   set ::mem(right5) [matrix_translation [expr -$::sllde5] 0 0]

   # draw robot ---------------------------------------
   set_worldmatrix [matrix_translation 200 0 150]
   mult_worldmatrix $::mem(link0)
   # draw base
   draw_prism base 40 50 40  -40 50 0

   mult_worldmatrix [matrix_translation 5 75 0]
   mult_worldmatrix $::mem(link1)
   # draw link 1
   draw_prism part1  5 10 10  0 0 0
   draw_prism part2 10 10 50   15 0 30

   mult_worldmatrix [matrix_translation  0 0 80]
   mult_worldmatrix $::mem(link2)
   # draw link 2
   draw_prism part3  5 10 10  0 0 0
   draw_prism part4 10 10 50   -15 0 30

   mult_worldmatrix [matrix_translation 0 0 80]
   mult_worldmatrix $::mem(link3)
   # draw link 3
   draw_prism part5  5 10 10  0 0 0
   draw_prism part6 10 10 50   15 0 30


   mult_worldmatrix [matrix_translation 15 0 85]
   mult_worldmatrix $::mem(link4)
   draw_prism part7 10 10 5  0 0 0 
   draw_prism part8 60 10 10  0 0 15

   set savemat [get_worldmatrix]  ;# save current matrix

   mult_worldmatrix $::mem(left5)
   draw_prism lefthand  10 10 50   10 0 75

   set_worldmatrix $savemat       ;# restore previous matrix
   mult_worldmatrix $::mem(right5)
   draw_prism righthand 10 10 50  -10 0 75
 }

 proc build_robot_controls { master } {
   set frame0 [labelframe $master.robot -text "Robot Controls"]

   grid \
     [create_scale base  $frame0 ::theta_base   180 -180  0] \
     [create_scale arm1  $frame0 ::anngle1 180 -180  0] \
     [create_scale arm2  $frame0 ::anngle2 180 -180  0] \
     [create_scale arm3  $frame0 ::anngle3 180 -180  0] \
     -sticky news

   grid \
     [create_scale handspin $frame0 ::theta_hand 90 -90  0] \
     [create_scale grasp    $frame0 ::sllde5 40 0 40] \
     -sticky news

   return $frame0
 }

 proc build_camera_controls { master } {
   set f [labelframe $master.camera -text "Camera Controls"]
   grid \
     [create_scale psi $f ::cam_angle1  180 -180  -22] \
     [create_scale phi $f ::cam_angle2  180 -180 -150] \
     -sticky news
   return $f
 }


 proc configure_drawing {} {
   $::maincanvas itemco WorkSpace -fill #00bb00
   $::maincanvas itemco Robot -width 2

   $::maincanvas itemco XAXIS -fill red
   $::maincanvas itemco YAXIS -fill #008800
   $::maincanvas itemco ZAXIS -fill blue
 }

 proc widget_setup_defaults {} {
   set ::screencenter_x 300
   set ::screencenter_y 400
   set ::perspective 1

   option add *Workspace.text                "Camera"
   option add *Robot.text                "Robot"
 }

 main_program