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