Moving an anthropomorphous robot arm
Marco Maggi On my 1.3 GHz processor I see a little delay while the event loop is going...
# 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:See also hoco an homogeneous coordinates package.
See another version at Simplified Robot Arm.
KPV See also Smallest Enclosing Disc which will tell you where to place the robot arm.