Manipulators

documented by Hiromu Onda

Instances of rotational-joint class and manipulator class constitute a Manipulator Model. rotational-joint is a subclass of the body. manipulator is a subclass of the cascaded-coords. rotational-joint class defines models of manipulator joints. manipulator class has methods for solving a forward kinematic solution and inverse kinematic solution.

The way of the definition of a manipulator is that i) Make all the joints of the manipulator, ii) Integrate these joints into manipulator.

Rotational Joint

rotational-joint describes a model of a joint. rotational-joint has body as super-class. This class manages a model of shape, coordinates, rotation axis of a joint, angles of rotation, limits of joint angles, etc. defjoint macro below creates an instance of rotational-joint. This instance is bound to joint-name. Assign a ancestor joint to parent. It is not necessary to assign rotational axes to base nor fingers.

defjoint   [macro] 

joint-name &key :shape  body-object 

:color 		 color-id   ;0-15 for MMD 

:parent 		 parent-joint 

:axis 		 rotational-axis 		  ; :x, :y or :z 

:offset 		 trans-from-parent-joint 

:low-limit 		 joint-angle-limit-low 

:high-limit 		 joint-angle-limit-hight

Defines a joint model.

Multi-Joint Manipulators

A model of a manipulator is described by manipulator. defmanipulator macro below creates an instance of manipulator.

defmanipulator   [macro] 

manipulator-name &key 

:class   manipulator-class 

:base 		 base-joint 

:joints 		  list-of-all-joints 

:hand 		 handjoint 

:left-finger 		 left-finger

:right-finger 		 right-finger

:handcoords 		 trans-from-hand-to-armsolcoords

:toolcoords 		 trans-from-armsolcoords-to-toolcoords 

:open-direction 		 finger-open-direction

:right-handed 		 righty-or-lefty

Defines a manipulator model.

rotational-joint [class]

  :super   body 

  :slots 		 (axis offset high-limit low-limit) 

describes each rotational joint of a 6 D.O.Fs manipulator.

manipulator [class]

  :super   cascaded-coords 

  :slots 		 (base baseinverse joint


angles right-handed hand handcoords right-finger left-finger


openvec max-span toolcoords toolinverse armsolcoords


toolinverse armsocoords approach grasp affix) 

manages kinematics of a manipulator from base to hand.

:newcoords newrot &optional newpos [method]

updates the coords with newrot and newpos if new joint angles are within the limit.

:armsolcoords [method]

computes and makes transformation (an instance coords) between the coords of the base and those of the hand.

:tool &rest msg [method]

modifies or gets toolcoords.

:set-tool newtool &optional offset copy [method]

sets new toolcoords.

:reset-tool [method]

forces this coords to be default-toolcoords.

:worldcoords [method]

computes the position vector, the rotation matrix, and the coordinates of the toolcoords represented in the world coordinates.

:set-coords [method]

forces setting coords according to the forward kinematic solution.

:config &optional (a newangles) [method]

sets joint angles of the manipulator.

:park [method]

forces all the joint angles to be zero.

:hand &optional (h nil) [method]

sets or returns the object of its hand.

:handcoords [method]

computes the position vector, the rotation matrix, and the coordinates of the handcoords represented in the world coordinates.

:span [method]

returns the current distance between fingers.

:open-fingers s &optional abs &aux (current (send self :span)) [method]

moves fingers relatively or absolutely.

:close-fingers [method]

closes fingers completely.

:angles &optional flag [method]

returns the list of current joint angles.

:get-approach [method]

returns the object to which the hand is approaching.

:set-approach a [method]

sets a as the object to which the hand will approach.

:get-grasp [method]

(:get-grasp () grasp-config)

:set-grasp g [method]

sets g as the object which the hand will grasp.

:get-affix [method]

returns the object which the hand grasps.

:affix &optional (grasp) [method]

sets grasp to affixed-object. grasp is associated to the handcoords as a descendant.

:unfix &optional (margin 10.0) [method]

sets affixed-object nil. grasp is dissociated (removed) from the descendants list of the handcoords.

:create   [method] 

&rest args 

&key (name nm) (hand h) (joints j)

(left-finger lf) (right-finger rf)

((toolcoords tc) (make-coords))

((handcoords hc) (make-coords))

((base bs) (make-cascoords))

(open-direction (floatvector 0 1 0))

((max-span mspan) 100.0)

((lefty lft) t)

((act a) nil)

&allow-other-keys

creates and initializes a new manipulator object.

manipulator manages the linkage of the coords of base, joints(J1...J6), handcoords, toolcoords. manipulator has cascaded-coords as super-class. manipulator is connected with base which is cascaded-coords (or subclasses of body). manipulator manages the transformation from the base frame to the toolcoords. Messages sent to manipulator (i.e. :translate, :locate, :rotate, :orient, :transform etc.) effect the end effector of the manipulator. If WRT parameter is set one of keywords (i.e. :local, :parent, :world or an instance of coordinates) in this message, the end-effector moves with respect to the WRT parameter. In the next program eta3 is a instance of manipulator.

　(send eta3 :translate #f(0 0 -100))        ;put back the end-effector by 10cm
　(send eta3 :translate #f(0 0 -100) :world) ;move down the end-effector by 10cm
　(send eta3 :translate #f(0 0 -100)
             (manipulator-base eta3))  ;move down the end-effector with respect 
                                       ;to the coords of the base by 10cm

When manipulator receives these messages, it calculates the arm solution and 6 joint angles are determined. Generally, more solutions than one exist. In that case, one appropriate solution is chosen of them according to the criteria (i.e. the distinction between right-handed and left-handed, and the consistency with current joint angles). If there is no solution for a given configuration or the calculated joint angles exceed its limits, manipulator does not move and it gives a warning.

Arm-solution method :armsol must be defined for respective manipulator classes which correspond to real manipulators. This method calculates the transformation between the base-coords and the hand-coords. Thus this allow us to put a manipulator wherever with respect to the world-coords. The arm solution is independent of the base, toolcoords.

Fig. 15 shows the relation between coordinate systems (base, J1, J2,..., handcoords and toolcoords). $T$ and other transformations are calculated as follows.

$$\begin{array}{ll} T & = base \cdot J1 \cdot J2 \cdot \ldots ... ...coords \\ & = (manipulator-handcoords \; eta3)\\ \end{array}$$

where $T$ is the transformation between the world-coords and the toolcoords.

Figure 15: relation between coordinate systems in a manipulator

Each joint has a geometric model represented by Breps (Boundary Representation). The coordinates of the vertices and the equations of the planes are not always current ones. Messages sent to manipulator for translation or rotation only update the coordinate systems, these do not update the coordinates of the vertices. This is why we can reduce the calculation time when translation or rotation occurs successively. If :worldcoords message is sent to manipulator, it updates the data such as the coordinates of the vertices.

Mainly toolcoords are used for specify the motion of a manipulator in this manipulator. There is a method (:config) for specifying the configuration of the manipulator by joint angles. The arguments are a float-vector whose elements are 6.

  (send eta3 :config (float-vector pi/2 pi/2 0 1 0 1))

:config rotates joints of the manipulator if the joint angles are in the limit. As a result, the coordinates which manipulator manages and the current toolcoords which given joint angles determines become inconsistent. :set-coords message must be sent if you need consistency. :set-coords calculates a forward kinematic solution and calculates the arm solution using the forward kinematic solution.

Example: create the manipulator model (ETA3) and draw this on a Xwindow system.

;EusLisp 7.27 with Xlib created on Thu Sep 17 14:33:30 1992
(load "view.l")                                ;open a window
(load "/usr/local/eus/robot/eta3/eta3build.l") ;create the model of ETA3
(send *viewing* :look #f(2000 2000 2000))      ;change the viewpoint
(send-all (eta3arm-components eta3) :color 1)  ;change the color of lines
(send eta3 :config (float-vector 0 (/ -Pi 4.0) Pi/2 0 (/ -Pi 4.0) 0 ))
					       ;set joint angles of ETA3
(send eta3 :set-coords)                        ;refer to the above explanation
(draw eta3)                                    ;draw ETA3