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