Version 0 of Moving an anthropomorphous robot arm

Updated 2004-11-27 08:22:29

# anthropomorphous-robot.tcl --

 # 
 # Part of: Useless Widgets Package
 # Contents: shows how to move an anthropomorphous robot arm
 # Date: Wed Nov 24, 2004
 # 
 # Abstract
 # 
 #        This script shows how to move linked elements with
 #        Denavit-Hartenberg coordinates transforms. It makes use of
 #        the "hoco.tcl" package, which you can find on the TCL'ers
 #        Wiki also. You have to place the "hoco.tcl" file in the
 #        same directory of this file.
 # 
 # Copyright (c) 2004 Marco Maggi
 # 
 # The author  hereby grant permission to use,  copy, modify, distribute,
 # and  license this  software  and its  documentation  for any  purpose,
 # provided that  existing copyright notices  are retained in  all copies
 # and that  this notice  is included verbatim  in any  distributions. No
 # written agreement, license, or royalty  fee is required for any of the
 # authorized uses.  Modifications to this software may be copyrighted by
 # their authors and need not  follow the licensing terms described here,
 # provided that the new terms are clearly indicated on the first page of
 # each file where they apply.
 # 
 # IN NO  EVENT SHALL THE AUTHOR  OR DISTRIBUTORS BE LIABLE  TO ANY PARTY
 # FOR  DIRECT, INDIRECT, SPECIAL,  INCIDENTAL, OR  CONSEQUENTIAL DAMAGES
 # ARISING OUT  OF THE  USE OF THIS  SOFTWARE, ITS DOCUMENTATION,  OR ANY
 # DERIVATIVES  THEREOF, EVEN  IF THE  AUTHOR  HAVE BEEN  ADVISED OF  THE
 # POSSIBILITY OF SUCH DAMAGE.
 # 
 # THE  AUTHOR  AND DISTRIBUTORS  SPECIFICALLY  DISCLAIM ANY  WARRANTIES,
 # INCLUDING,   BUT   NOT  LIMITED   TO,   THE   IMPLIED  WARRANTIES   OF
 # MERCHANTABILITY,    FITNESS   FOR    A    PARTICULAR   PURPOSE,    AND
 # NON-INFRINGEMENT.  THIS  SOFTWARE IS PROVIDED  ON AN "AS  IS" BASIS,
 # AND  THE  AUTHOR  AND  DISTRIBUTORS  HAVE  NO  OBLIGATION  TO  PROVIDE
 # MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
 #

 #page
 ## ------------------------------------------------------------
 ## Setup.
 ## ------------------------------------------------------------

 package require Tcl 8.4
 package require Tk  8.4

 set pathname [file dirname $argv]
 source [file join $pathname hoco.tcl]

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

 option add *topgeometry +20+20
 option add *borderWidth                        1
 option add *Labelframe.borderWidth        2
 option add *command_buttons.exit.text        "Exit"
 foreach { option value } {
     background \#f8f8f8 width 600 height 600 relief sunken borderwidth 2
     x_axis_color red y_axis_color blue z_axis_color green
 } { option add *Drawing.Canvas.$option $value }
 proc widget_option_scale_from_to { master from to } {
     option add *${master}.to        $to
     option add *${master}.from        $from
 }
 proc widget_option_scale_rotation { args } {
     foreach w $args { widget_option_scale_from_to $w 180.0 -180.0 }
 }
 proc widget_option_scale_translation { args } {
     foreach w $args { widget_option_scale_from_to $w 300.0 -300.0 }
 }
 #page
 ## ------------------------------------------------------------
 ## Widget procedures.
 ## ------------------------------------------------------------

 proc widget_grid_frames { args } { foreach w $args { grid $w -sticky news } }
 proc widget_configure_toplevel {} {
     wm geometry . [option get . topgeometry {}]
     wm title . [option get . toptitle {}]
     foreach event { <Return> <Escape> } { bind . $event main_exit }
 }
 proc widget_build_canvas { master } {
     global        widget_canvas
     set f [frame $master.drawing -class Drawing]
     grid [set widget_canvas [canvas $f.canvas]] -sticky news
     return $f
 }
 proc widget_build_command_buttons { master } {
     set f [frame $master.command_buttons]
     focus [grid [button $f.exit -command main_exit]]
     return $f
 }
 proc widget_build_scale_frame { master coord_spec } {
     set f [labelframe $master.$coord_spec -class [string totitle $coord_spec]]
     set column_index 0    
     foreach name [uwp_hoco_instance_get_dynamic_parameter_names $coord_spec] {
         label [set label_widget $f.lab_$name] -text [string totitle $name]
         scale [set scale_widget $f.$name]
         $scale_widget set \
             [uwp_hoco_instance_get_parameter_value $coord_spec $name]
         $scale_widget configure -command \
             [list widget_update_parameter_from_scale $coord_spec $name]
         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 widget_update_parameter_from_scale { coord_spec param_name param_value } {
     uwp_hoco_instance_update_parameter $coord_spec $param_name $param_value
     after 0 widget_put_drawing_on_canvas
 }
 proc widget_canvas_draw { command coords {main_tag {}} {tags {}} } {
     global        widget_canvas
     if { [string length $main_tag] } { $widget_canvas delete $main_tag }
     $widget_canvas create $command $coords -tags [lappend tags $main_tag]
 }
 proc widget_canvas_query_option { option } {
     global        widget_canvas
     option get $widget_canvas $option {}
 }
 proc widget_canvas_tag_config { tag args } {
     global        widget_canvas
     eval { $widget_canvas itemconfigure $tag } $args
 }

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

 proc main {} {
     global        exit_trigger

     uwp_hoco_instance_declare workspace -type workspace \
         -dynamic [uwp_hoco_transform_get_parameter_names workspace]
     uwp_hoco_instance_declare base -type homogeneous \
         -dynamic [uwp_hoco_transform_get_parameter_names workspace] \
         -parameters { 0 90 0 150 0 150 }

     widget_build_all

     uwp_hoco_instance_update_parameter canvas y 400
     uwp_wireframe_draw_reference_frame Canvas_Frame \
         {canvas} {yes {200 0 0 1  0 200 0 1  0 0 0 1  0 0 0 1}}

     widget_put_drawing_on_canvas
     interp alias {} main_exit {} uplevel \#0 {set exit_trigger 1}
     vwait exit_trigger
     exit
 }
 proc widget_put_drawing_on_canvas {} {
     draw_work_space
     draw_robot
     widget_canvas_configure_tags
 }
 #page
 ## ------------------------------------------------------------
 ## Work space proof widgets.
 ## ------------------------------------------------------------

 proc widget_build_all {} {
     widget_setup_options
     widget_configure_toplevel
     grid columnconfigure . 0 -weight 1
     grid rowconfigure . 0 -weight 1
     grid [widget_build_canvas .] [frame [set right_frame .right]] -sticky news
     widget_grid_frames \
         [widget_build_command_buttons $right_frame] \
         [widget_build_scale_frame $right_frame workspace] \
         [widget_build_robot_frame $right_frame]
 }
 proc widget_build_robot_frame { master } {
     set f [labelframe $master.robot -class Robot]
     grid \
         [widget_build_scale_frame $f link0] \
         [widget_build_scale_frame $f link1] \
         [widget_build_scale_frame $f link3] \
         [widget_build_scale_frame $f link5] \
         -sticky news
     grid \
         [widget_build_scale_frame $f link8] \
     [widget_build_end_effector_frame $f link10 link11] \
         -sticky news
     return $f
 }
 proc widget_build_end_effector_frame { master left right } {
     set f [labelframe $master.endeffector -class End_effector]
     label [set label_widget $f.lab_grab] -text "End effector"
     scale [set scale_widget $f.grab]
     $scale_widget set [uwp_hoco_instance_get_parameter_value $left a]
     $scale_widget configure -command \
         [list widget_update_end_effector $left $right a]
     grid $label_widget -row 0 -sticky news
     grid $scale_widget -row 1 -sticky news
     return $f
 }
 proc widget_update_end_effector { left right coord value } {
     uwp_hoco_instance_update_parameter $left $coord $value
     uwp_hoco_instance_update_parameter $right $coord [expr {-($value)}]
     after 0 widget_put_drawing_on_canvas
 }
 proc widget_canvas_configure_tags {} {
     foreach axis {x y z} {
         widget_canvas_tag_config reference_frame_${axis}axis \
             -fill [widget_canvas_query_option ${axis}_axis_color] }
     foreach arglist {
         {reference_frame -arrow last} {Canvas_Frame -fill "\#d0d0d0"}
     } { eval widget_canvas_tag_config $arglist }
     widget_canvas_tag_config WorkSpace -fill green
     widget_canvas_tag_config Robot -width 2
 }
 proc widget_setup_options {} {
     option add *toptitle "Moving an anthropomorphous robot arm"
     widget_option_scale_rotation \
         workspace.phi workspace.psi \
         link0.theta link1.alpha link3.alpha link5.alpha link8.theta
     foreach widget { endeffector.grab } {
         option add *${widget}.to        0
         option add *${widget}.from        -20
     }
     foreach {name text} { Workspace "Work Space" Robot "Robot" } {
         option add *$name.text                $text
         option add *$name.borderWidth        2
     }
 }
 #page
 ## ------------------------------------------------------------
 ## Graphical elements.
 ## ------------------------------------------------------------

 interp alias {} draw_work_space {} uwp_wireframe_draw_workspace \
     WorkSpace { workspace canvas }

 #page
 ## ------------------------------------------------------------
 ## Robot links.
 ## ------------------------------------------------------------

 # base link, vertical rotation
 uwp_hoco_instance_declare link0 -type dh -dynamic { theta }
 interp alias {} draw_link_0 {} uwp_wireframe_draw_prism link0 \
     { link0 base workspace world canvas } \
     { yes { 20 0 0 0  0 20 0 0  0 0 50 50  0 0 0 1 } } Robot
 # rotational link
 uwp_hoco_instance_declare link1 -type dh -dynamic { alpha } -parameters {80 0 25 0}
 interp alias {} draw_link_1 {} uwp_wireframe_draw_prism link1 \
     { link1 link0 base workspace world canvas } \
     { yes { 5 0 0 0  0 20 0 0  0 0 20 0  0 0 0 1 } } Robot
 # fixed link
 uwp_hoco_instance_declare link2 -type dh -dynamic {}
 interp alias {} draw_link_2 {} uwp_wireframe_draw_prism link2 \
     { link2 link1 link0 base workspace world canvas } \
     { yes { 20 0 0 25  0 20 0 0  0 0 50 30  0 0 0 1 } } Robot
 # rotational link
 uwp_hoco_instance_declare link3 -type dh -dynamic { alpha } -parameters {60 0 0 0}
 interp alias {} draw_link_3 {} uwp_wireframe_draw_prism link3 \
     { link3 link2 link1 link0 base workspace world canvas } \
     { yes { 5 0 0 0  0 20 0 0  0 0 20 0  0 0 0 1 } } Robot
 # fixed link
 uwp_hoco_instance_declare link4 -type dh -dynamic {}
 interp alias {} draw_link_4 {} uwp_wireframe_draw_prism link4 \
     { link4 link3 link2 link1 link0 base workspace world canvas } \
     { yes { 20 0 0 -25  0 20 0 0  0 0 50 30  0 0 0 1 } } Robot
 # rotational link
 uwp_hoco_instance_declare link5 -type dh -dynamic { alpha } -parameters {60 0 0 0}
 interp alias {} draw_link_5 {} uwp_wireframe_draw_prism link5 \
     { link5 link4 link3 link2 link1 link0 base workspace world canvas } \
     { yes { 5 0 0 0  0 20 0 0  0 0 20 0  0 0 0 1 } } Robot
 # fixed link
 uwp_hoco_instance_declare link6 -type dh -dynamic {}
 interp alias {} draw_link_6 {} uwp_wireframe_draw_prism link6 \
     { link6 link5 link4 link3 link2 link1 link0 base workspace world canvas } \
     { yes { 20 0 0 25  0 20 0 0  0 0 50 30  0 0 0 1 } } Robot
 # transparent link
 uwp_hoco_instance_declare link7 -type dh -dynamic {} -parameters {85 0 25 0}
 interp alias {} draw_link_7 {} uwp_wireframe_draw_prism link7 \
     { link7 link6 link5 link4 link3 link2 link1 link0 base workspace world canvas } \
     { yes { 0 0 0 0  0 0 0 0  0 0 0 0  0 0 0 1 } } Robot
 # rotational link
 uwp_hoco_instance_declare link8 -type dh -dynamic { theta } -parameters {0 0 0 0}
 interp alias {} draw_link_8 {} uwp_wireframe_draw_prism link8 \
     { link8 link7 link6 link5 link4 link3 link2 link1 link0 base workspace world canvas } \
     { yes { 20 0 0 0  0 20 0 0  0 0 5 0  0 0 0 1 } } Robot
 # fixed link, end-effector base
 uwp_hoco_instance_declare link9 -type dh -dynamic {}
 interp alias {} draw_link_9 {} uwp_wireframe_draw_prism link9 \
     { link9 link8 link7 link6 link5 link4 link3 link2 link1 link0
         base workspace world canvas } \
     { yes { 60 0 0 0  0 20 0 0  0 0 20 25  0 0 0 1 } } Robot
 # translating link, end-effector theet
 uwp_hoco_instance_declare link10 -type dh -dynamic { a }
 interp alias {} draw_link_10 {} uwp_wireframe_draw_prism link10 \
     { link10 link9 link8 link7 link6 link5 link4 link3 link2 link1 link0
         base workspace world canvas } \
     { yes { 20 0 0 40  0 20 0 0  0 0 30 75  0 0 0 1 } } Robot
 uwp_hoco_instance_declare link11 -type dh -dynamic {}
 interp alias {} draw_link_11 {} uwp_wireframe_draw_prism link11 \
     { link11 link9 link8 link7 link6 link5 link4 link3 link2 link1 link0
         base workspace world canvas } \
     { yes { 20 0 0 -40  0 20 0 0  0 0 30 75  0 0 0 1 } } Robot

 proc draw_robot {} { for {set i 0} {$i < 12} {incr i} { draw_link_$i } }

 #page
 ## ------------------------------------------------------------
 ## Let's go.
 ## ------------------------------------------------------------

 main

 ### end of file
 # Local Variables:
 # mode: tcl
 # End: