## Version 1 of Simplified Robot Arm

Updated 2007-03-04 06:13:50

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.
## ------------------------------------------------------------

## ------------------------------------------------------------
## 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_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(left5)  [matrix_translation        \$::sllde5  0 0]
set ::mem(right5) [matrix_translation [expr -\$::sllde5] 0 0]

# draw robot ---------------------------------------
set_worldmatrix [matrix_translation 200 0 150]
# draw base
draw_prism base 40 50 40  -40 50 0

mult_worldmatrix [matrix_translation 5 75 0]
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]
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]
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]
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