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