# denavit-hartenberg.tcl --
#
# Part of: Useless Widgets Package
# Contents: shows how to move a frame along Denavit-Hartenberg coords
# Date: Tue Nov 2, 2004
#
# Abstract
#
# This script is just a proof.
#
# 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
#page
## ------------------------------------------------------------
## Global variables.
## ------------------------------------------------------------
# theta -> counterclockwise rotation of the frame center,
# origin is the horizontal axis left->right
# x -> horizontal coordinate of the frame center
# y -> vertical of the frame center
#
# (0,0) --> TK x coordinate
# ------------------------------- -
# TK | ^ / | : y parameter
# y | |/ \theta param | :
# coord | -----+-----> | -
# | | | |
# v | | |
# -------------------------------
#
# |...............| x parameter
#
set canvas_coord_names {theta x y}
set canvas_parameters {0 250 250}
set canvas_matrix {1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1}
# theta -> rotation around the world's Z axis
# psi -> rotation around the world's X axis
# phi -> rotation around the world's Y axis
set world_coord_names { theta phi psi }
set world_parameters {0 0 0}
set world_frame {}
set world_matrix {1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1}
# d -> translation along the world Z axis
# theta -> rotation around the world Z axis (in degrees)
# a -> translation along the D-H X axis
# alpha -> rotation around the D-H X axis (in degrees)
set dh_coord_names { d theta a alpha }
set dh_coord_to_radiant { no yes no yes }
set dh_parameters {0 0 0 0}
set dh_frame {}
set dh_matrix {1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1}
set pi 3.14159265359
set pi_over_180 [expr {$pi/180.0}]
set 180_over_pi [expr {180.0/$pi}]
#page
## ------------------------------------------------------------
## TK options.
## ------------------------------------------------------------
option add *borderWidth 1
option add *Labelframe.borderWidth 2
foreach { option value } {
background \#f8f8f8 width 500 height 500 relief sunken borderwidth 2
} {
option add *c.$option $value
}
set delta 200.0
option add *dh.d.to -200.0
option add *dh.d.from 200.0
option add *dh.theta.to -180.0
option add *dh.theta.from 180.0
option add *dh.a.to -200.0
option add *dh.a.from 200.0
option add *dh.alpha.to -180.0
option add *dh.alpha.from 180.0
foreach coord $world_coord_names {
option add *world.$coord.to -180.0
option add *world.$coord.from 180.0
}
#page
## ------------------------------------------------------------
## Widgets.
## ------------------------------------------------------------
proc widget_build {} {
global world_coord_names dh_coord_names world_frame dh_frame
wm geometry . +20+20
wm title . "Playing with Denavit and Hartenberg"
bind . <Return> {set exit_trigger 1}
bind . <Escape> {set exit_trigger 1}
set s [frame .scales]
button $s.exit -text Exit -command {set exit_trigger 1}
widget_build_scale_frame $s world $world_coord_names "World Frame"
widget_build_scale_frame $s dh $dh_coord_names "Denavit-Hartenberg"
set f [frame $s.explain_colors]
foreach {ax color} { xaxis red yaxis blue zaxis green} {
label $f.$ax -text $ax -foreground $color
}
grid $f.xaxis $f.yaxis $f.zaxis -sticky news
grid $s.exit
grid $f
grid $world_frame -sticky news
grid $dh_frame -sticky news
grid columnconfigure . 0 -weight 1
grid rowconfigure . 0 -weight 1
grid [canvas .c] $s
focus $s.exit
}
proc widget_build_scale_frame { master name coord_names label } {
set var_name [format "%s_frame" $name]
upvar \#0 $var_name frame
set frame [labelframe $master.$name -text $label]
widget_build_scales $frame $coord_names [format "update_%s_matrix_" $name]
}
proc widget_build_scales { master coord_names update_command_prefix } {
set column_index 0
foreach coord $coord_names {
set labw $master.lab_$coord
set scaw $master.$coord
label $labw -text $coord
scale $scaw -command [format "%s%s" $update_command_prefix $coord]
grid $labw -column $column_index -row 0 -sticky news
grid $scaw -column $column_index -row 1 -sticky news
incr column_index
}
}
proc widget_canvas_configure_tags {} {
# Axis settings are overridden by frame settings.
foreach {tag color} {xaxis red yaxis blue zaxis green} {
.c itemconfigure $tag -fill $color
}
.c itemconfigure World_Frame -dash ,
#.c itemconfigure Dh_Frame -nooption novalue
.c itemconfigure Canvas_Frame -fill black
.c itemconfigure Frame -arrow last
}
#page
proc math_deg2rad { angle } {
global pi_over_180
expr {double($angle)*$pi_over_180}
}
proc math_rad2deg { angle } {
global 180_over_pi
expr {double($angle)*$180_over_pi}
}
proc math_sin_cos { angle nickname } {
upvar cos_$nickname cos_t sin_$nickname sin_t
set cos_t [expr {cos(double($angle))}]
set sin_t [expr {sin(double($angle))}]
}
#page
proc dh_parameters_to_trasformation_matrix { parameters } {
global dh_coord_names
foreach $dh_coord_names $parameters {}
math_sin_cos $theta t
math_sin_cos $alpha a
math_compute_transformation_matrix {
{$cos_t} {-1.0*($cos_a*$sin_t)} {$sin_a*$sin_t} {double($a)*$cos_t}
{$sin_t} {$cos_a*$cos_t} {-1.0*($sin_a*$cos_t)} {double($a)*$sin_t}
0 {$sin_a} {$cos_a} {double($d)}
} 0 0 0 1
}
proc world_parameters_to_transformation_matrix { parameters } {
global world_coord_names
foreach $world_coord_names $parameters {}
math_sin_cos $theta t
math_sin_cos $phi f
math_sin_cos $psi p
math_compute_transformation_matrix {
{$cos_t*$cos_f-($sin_f*$sin_t*$sin_p)} {-($sin_t*$cos_p)}
{$cos_t*$sin_f+$sin_t*$cos_f*$sin_p} 0
{$sin_t*$cos_f+$cos_t*$sin_p*$sin_f} {$cos_t*$cos_p}
{$sin_t*$sin_f-($cos_t*$cos_f*$sin_p)} 0
{-($cos_p*$cos_f)} {$sin_p} {$cos_p*$cos_f} 0
} 0 0 0 1
}
proc canvas_parameters_to_transformation_matrix { parameters } {
global canvas_coord_names
foreach $canvas_coord_names $parameters {}
math_sin_cos $theta t
math_compute_transformation_matrix {
{$cos_t} {-$sin_t} 0 {$x}
{-$sin_t} {-$cos_t} 0 {$y}
} 0 0 1 0 0 0 0 1
}
proc math_compute_transformation_matrix { expr_list args } {
set matrix {}
foreach expr $expr_list { lappend matrix [uplevel expr $expr] }
if { [llength $args] } {
eval { lappend matrix } $args
} else {
set matrix
}
}
#page
proc initialise_canvas_matrix { parameters } {
upvar \#0 canvas_matrix matrix
set matrix [canvas_parameters_to_transformation_matrix $parameters]
}
proc update_dh_matrix { index rad value } {
upvar \#0 dh_parameters parameters dh_matrix matrix
if { $rad } { set value [math_deg2rad $value] }
lset parameters $index $value
set matrix [dh_parameters_to_trasformation_matrix $parameters]
after 0 update_drawing
}
foreach name $dh_coord_names rad $dh_coord_to_radiant {
interp alias {} update_dh_matrix_$name \
{} update_dh_matrix [lsearch $dh_coord_names $name] $rad
}
proc update_world_matrix { index value } {
upvar \#0 world_parameters parameters world_matrix matrix
lset parameters $index [math_deg2rad $value]
set matrix [world_parameters_to_transformation_matrix $parameters]
after 0 update_drawing
}
interp alias {} update_world_matrix_theta {} update_world_matrix 0
interp alias {} update_world_matrix_phi {} update_world_matrix 1
interp alias {} update_world_matrix_psi {} update_world_matrix 2
#page
proc transformation { matrices coords } {
foreach matrix $matrices {
foreach {
a11 a12 a13 a14 a21 a22 a23 a24 a31 a32 a33 a34 a41 a42 a43 a44
} $matrix {}
set result {}
foreach {x y z other} $coords {
lappend result \
[expr {double($x)*double($a11)+
double($y)*double($a12)+
double($z)*double($a13)+
double($other)*double($a14)}] \
[expr {double($x)*double($a21)+
double($y)*double($a22)+
double($z)*double($a23)+
double($other)*double($a24)}] \
[expr {double($x)*double($a31)+
double($y)*double($a32)+
double($z)*double($a33)+
double($other)*double($a34)}] \
[expr {double($x)*double($a41)+
double($y)*double($a42)+
double($z)*double($a43)+
double($other)*double($a44)}]
}
set coords $result
}
set result {}
foreach { x y z o } $coords { lappend result $x $y }
return $result
}
#page
proc update_drawing {} {
draw_world_frame
draw_dh_frame
widget_canvas_configure_tags
}
interp alias {} update_dh_scales {} update_scales dh_frame $dh_coord_names
interp alias {} update_world_scales {} update_scales world_frame $world_coord_names
proc update_scales { frameVar coordNames coordValues } {
upvar \#0 $frameVar frame
foreach name $coordNames value $coordValues { $frame.$name set $value }
}
#page
interp alias {} draw_canvas_frame {} draw_frame Canvas_Frame \
{canvas_matrix} \
{ -100 0 0 1 100 0 0 1 } \
{ 0 -100 0 1 0 100 0 1 } \
{ 0 0 0 1 0 0 0 1 }
interp alias {} draw_dh_frame {} draw_frame Dh_Frame \
{dh_matrix world_matrix canvas_matrix} \
{ -100 0 0 1 100 0 0 1 } \
{ 0 -100 0 1 0 100 0 1 } \
{ 0 0 -100 1 0 0 100 1 }
interp alias {} draw_world_frame {} draw_frame World_Frame \
{world_matrix canvas_matrix} \
{ 0 0 0 1 100 0 0 1 } \
{ 0 0 0 1 0 100 0 1 } \
{ 0 0 0 1 0 0 100 1 }
proc draw_frame { tag matrix_names xaxis yaxis zaxis } {
set matrices [lmap {uplevel \#0 set} $matrix_names]
.c delete $tag
foreach varname { xaxis yaxis zaxis } {
set id [.c create line [transformation $matrices [set $varname]]]
.c addtag $tag withtag $id
.c addtag Frame withtag $id
.c addtag $varname withtag $id
}
}
proc lmap { cmd lst } {
foreach item $lst { lappend result [eval $cmd { $item }] }
set result
}
#page
## ------------------------------------------------------------
## Do stuff.
## ------------------------------------------------------------
proc main {} {
global exit_trigger \
canvas_parameters world_parameters dh_parameters
widget_build
initialise_canvas_matrix $canvas_parameters
draw_canvas_frame
update_world_scales $world_parameters
update_dh_scales $dh_parameters
vwait exit_trigger
exit
}
#page
## ------------------------------------------------------------
## Let's go.
## ------------------------------------------------------------
main
### end of file
# Local Variables:
# mode: tcl
# End:
The Denavit-Hartenberg representation is a standard identification in robotics of joints and orthonormal (x, y, z) coordinate systems, one particularly useful for describing the space of all controlled motions.