Embed Size (px)
Citation preview
Planning Motions with Intentions
Yoshihito Kogay, Koichi Kondoz, James Kuffnery and Jean-Claude Latombey
y Robotics Laboratory, Department of Computer Science, Stanford UniversityStanford, CA 94305, USA
z R & D Center, Toshiba Corporation, 4-1 Ukishima-cho, Kawasaki, 210, Japan
Abstract
We apply manipulation planning to computer animation. Anew path planner is presented that automatically computesthe collision-free trajectories for several cooperating armsto manipulate a movable object between two configurations.This implemented planner is capable of dealing with com-plicated tasks where regrasping is involved. In addition, wepresent a new inverse kinematics algorithm for the humanarms. This algorithm is utilized by the planner for the gen-eration of realistic human arm motions as they manipulateobjects. We view our system as a tool for facilitating theproduction of animation.
Keywords: Task-level graphic animation, automatic ma-nipulation planning, Human arm kinematics.
1 Introduction
Mundane details keep us from vigorously attacking big-ger ideas. This is the motivation for achieving task-levelanimation. From task-level descriptions, the animation offigures in a scene can be automatically computed by anappropriate motion planner. The animator can therebyconcentrate on creating imaginative graphics, rather thanlabouring over the chore of moving these figures in a re-alistic and collision-free manner. In this paper we present
our efforts towards realizing a subset of this ultimate goal -the automatic generation of human and robot arm motionsto complete manipulation tasks.
Why study manipulation with arms? Human figures of-ten play an integral role in computer animation. Conse-quently, there are arm motions and more specifically ma-nipulation motions to animate. Another major applicationis in ergonomics. Since most products are utilized, assem-bled, maintained, and repaired by humans, and require formost cases some action by the human arms, by simulatingand viewing these arm motions through computer graphics,one can evaluate the design of the product in terms of itsusability. This will reduce the number of mock-up modelsneeded to come up with the final design. Again, this wouldallow the designer more time towards creating high-qualityproducts.
Unlike the motion of passive systems like falling objectsor bouncing balls, the motion of human and robot arms forthe purpose of manipulation are “motions with intentions”.The arms move not through some predictable trajectory dueto the laws of physics (as is the case with a falling object[2, 9]) but with the intention of completing some task. Aplanner is needed to determine how the arms must moveto complete the task at hand. Although there has beenprevious work on simulating walking and lifting motions,this is the first attempt to automatically generate complexmanipulation motions.
Our problem is thus to find a collision-free path for thearms to grasp and then carry some specified movable objectfrom its initial location to a desired goal location. Thisproblem is known as the multi-arm manipulation planningproblem [12, 7]. A crucial difference, relative to more clas-sical path planning, is that we must account for the abilityof the arms to change their grasp of the object. Indeed,for some tasks the arms may need to ungrasp the objectand regrasp it in a new manner to successfully complete themotion.
We present a new planner that solves this multi-arm ma-nipulation problem [12]. The planner needs as input thegeometry of the environment, the initial and goal configu-
rations of the movable object and arms, a set of potentialgrasps of the movable object, and the inverse kinematicsof the arms. With appropriate book-keeping, the animatorwould simply specify the goal configuration of the mov-able object (a task-level description) to generate the desiredanimation.
The planning approach is flexible in regards to the armtypes that can be considered. The only restriction is that thearms must have an inverse kinematics algorithm. Thus, inaddition to the planner, we present a new inverse kinematicsalgorithm for the human arms based on results from neu-rophysiology. This algorithm resolves the redundancy ofthe human arms by utilizing a sensorimotor transformationmodel [29, 30]. The result is the automatic animation ofhuman arm manipulation tasks.
In addition to the motion planning aspect, we addressthe issue of producing natural motions when human figuresare animated. By applying results from neurophysiologyto various parts of the planner, for instance the inversekinematics, we hope to achieve a good approximation ofnaturalness. We believe that the geometry of the motionsproduced by the planner are in fact quite natural.1
Fig. 1 is a series of snapshots along a manipulation pathcomputed by our planner. Once the necessary models ofthe environment are read by the planner, the input from theanimator is simply the goal location for the eye glasses,in this case getting placed on the head. Note, the plannerfound automatically that the arms must ungrasp and regraspthe glasses in order to complete the task.
Section 2 relates our efforts to previous work. Section 3describes the details of our motion planner. Section 4 is thederivation of the human arm inverse kinematics. Section5 is a discussion of how our system takes natural humanarm motion into consideration. Finally, Section 6 presentsresults obtained with the planner.
2 Related Work
Planning motions with intentions for robot and human armmanipulation is related to several different areas of research.We roughly classify this related work into three categories,manipulation planning, animation of human figures, andneurophysiology.
Manipulation Planning: The use of path planning toautomatically generate graphic animation was already sug-gested in [18]. Research strictly addressing manipulationplanning is fairly recent. The first paper to tackle this prob-lem is by Wilfong [34]. It considers a single-body robottranslating in a 2D workspace with multiple movable ob-jects. The robot, the movable objects and obstacles are
1This paper does not consider the velocity distribution along theplanned motions.
modelled as convex polygons. In order for the movable ob-jects to reach their specified goal the robot must “grasp” andcarry them there. Wilfong shows that planning a manipu-lation path to bring the movable objects to their specifiedgoal locations is PSPACE-hard. When there is a single mov-able object, he proposes a complete algorithm that runsin O�n3 log2 n� time, where n is the total number of ver-tices of all the objects in the environment. Laumond andAlami [15] propose an O�n4� algorithm to solve a similarproblem where the robot and the movable object are bothdiscs and the obstacles are polygonal.
Our work differs from this previous research in severalways. Rather than dealing with a single robot, we considerthe case of multiple human and robot arms manipulatingobjects in a 3D workspace. In addition, whereas the previ-ous work is more theoretical in nature, our focus is more ondeveloping an effective approach to solving manipulationtasks of a complexity comparable to those encountered ineveryday situations (e.g. picking and placing objects on atable).
Regrasping is a vital component in manipulation tasks.Tournassoud, Lozano-Perez, and Mazer [32] specificallyaddress this problem. They describe a method for plan-ning a sequence of regrasp operations by a single arm tochange an initial grasp into a goal grasp. At every regrasp,the object is temporarily placed on a horizontal table in astable position selected by the planner. We too need to planregrasp operations. However, the only regrasping motionswe consider avoid contact between the object and the envi-ronment; they necessarily involve multiple arms (e.g. bothhuman arms).
Grasp planning is potentially an important componentof manipulation planning. In our planner, grasps are onlyselected from a finite predefined set. An improvement forthe future will be to include the automatic computation ofgrasps. A quite substantial amount of research has beendone on this topic. See [22] for a commented list of biblio-graphical references.
Animation of Human Figures: Human gaits have beensuccessfully simulated. For example, Bruderlinand Calvert[5] have proposed a hybrid approach to the animation ofhuman locomotion which combines goal-directed and dy-namic motion control. McKenna and Zeltzer [21] have suc-cessfully simulated the gait of a virtual insect by combin-ing forward dynamic simulation and a biologically-basedmotion coordination mechanism. Control algorithms havebeen successfully applied to the animation of dynamiclegged locomotion [25]. While dynamic models and theuse of motor coordination models have been successfullyapplied to a wide range of walking motions, such a strategyhas yet to be discovered to encompass human arm motions.
Figure 1. Planned path for manipulating eye glasses.
For simulating the motion of human arms, there existmethods for specific tasks. For example, Lee et al. [17]have focused on the simulation of arm motions for liftingbased on human muscle models.
Their method considers such factors as comfort level, per-ceived exertion, and strength. Our approach is to simulatenatural arm motions from the point of view of kinematics,that is we make no consideration of dynamics and musclemodels. We justify this approach in Section 5.
There has been previous work in applying motion plan-ning algorithms to animating human figures. Ching andBadler [6] present a motion planning algorithm for an-thropometric figures with many degrees of freedom. Es-sentially, they use a sequential search strategy to find acollision-free motion of the figure to a specified goal con-figuration. They do not consider manipulation or imposingnaturalness on the motions.
The AnimNL project at the University of Pennsylvania[33] is working to automate the generation of human figuremovements from natural language instructions. Their focusis mainly on determining the sequence of primitive actionsfrom a high-level description of the task. They utilize mod-els to create realistic motions, however they do not considercomplex manipulation motions.
Neurophysiology: There are many scientists in psy-chology and neurophysiology working to determine howthe human brain manages to coordinate the motion of itslimbs. Soechting [31] gives a good survey of various em-pirical studies and their results for human arm motions.
One relevant finding is the sensorimotor transformationmodel devised by Soechting and Flanders [29, 30]. Theyfound that the desired position of the hand roughly deter-mines the arm posture. Our inverse kinematics algorithmfor the human arm is based on this result.
3 Manipulation Planner
In this section we present our manipulation planning algo-rithm. The method applies to any system of arms as longas they have an inverse kinematics solution.
3.1 Problem Statement
We now give a rather formal formulation of the multi-armmanipulation planning problem. We consider only a singlemovable object, but for the rest, our presentation is general.
The environment is a 3D workspace W with p arms Ai
(i � 1� � � � � p), a single movable object M, and q staticobstacles Bj (j � 1� � � � � q). The objectM can only moveby having one or several of the arms grasp and carry it tosome destination.
Let Ci and Cobj be the C-spaces (configuration spaces)of the arms Ai and the object M, respectively [19, 16].
Each Ci has dimension ni, where ni is the number ofdegrees of freedom of the arm Ai, and Cobj has dimen-sion 6. The composite C-space of the whole system isC � C1 � � � � � Cp � Cobj. A configuration in C, calleda system configuration, is of the form �q1� � � � � qp� qobj�,with qi � Ci and qobj � Cobj.
We define the C-obstacle region CB � C as the set ofall system configurations where two or more bodies infA1� � � � �Ap�M�B1� � � � �Bqg intersect.2 We describe allbodies as closed subsets ofW; hence, CB is a closed subsetof C. The open subset C n CB is denoted by Cfree and itsclosure by cl�Cfree�.
For the most part we require that the arms, object, and ob-stacles do not contact one another. However,Mmay touchstationary arms and obstacles for the purpose of achievingstatic stability. M may also touch arms when it is be-ing moved. This is to achieve grasp stability and M canonly make contact with the end-effector of each graspingarm (grasping may involve one or several arms). No othercontacts are allowed.
This leads us to define two subsets of cl�Cfree�:- The stable space Cstable is the set of all legal configurationsin cl�Cfree�whereM is statically stable. M’s stabilitymaybe achieved by contacts between M and the arms and/orthe obstacles.- The grasp space Cgrasp is the set of all legal configurationsin cl�Cfree� where one or several arms rigidly grasp M insuch a way that they have sufficient torque to move M.Cgrasp � Cstable.
There are two types of paths, transit and transfer paths,which are of interest in multi-arm manipulation:- A transit path defines arm motions that do not move M.Along such a pathM’s static stability must be achieved bycontacts with obstacles and/or stationary arms. Examplesof such a path involve moving an arm to a configurationwhere it can graspM or moving an arm to change its graspof M. A transit path lies in the cross-section of Cstabledefined by the current fixed configuration ofM.- A transfer path defines arm motions that move M. Itlies in the cross-section of Cgrasp defined by the attachmentof M to the last links of the grasping arms. During atransfer path, not all moving arms need grasp M; somenon-grasping arms may be moving to allow the graspingarms to move without collision.
A manipulation path is an alternate sequence of transitand transfer paths that connects an initial system configura-tion, qi
sys, to a goal system configuration, qgsys (see Fig. 2).Some paths in this sequence may be executed concurrentlyas long as this does not yield collisions.
2We regard joint limits in Ai as obstacles that only interfere with thearms’ motions.
Grasp Space
Stable Space
Transit Path
Transfer PathGrasp Space
Grasp Space
qisys
qsysg
cl(Cfree )
Figure 2. Components of a manipulation path and theirrelation to the subspaces of cl�Cfree�.
In a multi-arm manipulation planning problem, the ge-ometry of the arms, movable object, and obstacles is given,along with the location of the obstacles. The goal is tocompute a manipulation path between two input systemconfigurations.
3.2 Planning Approach
We now describe our approach for solving the multi-armmanipulation planning problem. This approach embedsseveral simplifications. As a result the corresponding plan-ner is not fully general. Throughout our presentation, wecarefully state the simplifications that we make. Some ofthem illustrate the deep intricacies of multi-arm manipula-tion planning.
Overview: A manipulation path alternates transit andtransfer paths. Each path may be seen as the plan fora subtask of the total manipulation task. This yields thefollowing two-stage planning approach: first, generate aseries of subtasks to achieve the system goal configuration;second, plan a transit or transfer path for each subtask. Aninformal example of a series of subtasks is: grabM, carryit to an intermediate location, change grasp, carry M to itsgoal location, ungrasp.
Unfortunately, planning a series of subtasks that can laterbe completed into legal paths is a difficult problem. Howcan one determine whether a subtask can be completedwithoutactually completing it? We settle for a compromise.Our approach focuses on planning a sequence of transfertasks that are guaranteed to be completed into transfer paths.In fact, in the process of identifying these tasks, the planneralso generates the corresponding transfer paths. With thetransfer tasks specified, the transit tasks are immediatelydefined: they link the transfer paths together along with theinitial and goal system configuration. It only remains tocompute the corresponding transit paths.
The assumption underlying this approach is that thereexists a legal transit path for every transit task. Actually, ina 3D workspace, this is often the case. If the assumption isnot verified, the planner may try to generate another series
of transfer tasks, but in our current implementation it simplyreturns failure.
Restrictions on grasps: To simplify the selection oftransfer tasks, we impose a restriction on grasps. The var-ious possible grasps of M are given as a finite grasp set.Each grasp in this set describes a rigid attachment of theend-effector(s) of one or several arms with M. For ex-ample, if M is considered too heavy or too bulky to bemoved by a single arm, each grasp in the grasp set mayindicate the need for a two-arm grasp. If the end-effectorconsists of multi-joint fingers, their joint values to achievethe particular grasp are specified.
Generating transfer tasks: The generation of the trans-fer tasks is done by planning a path �obj ofM from its initialto its goal configuration. During the computation of �obj,all the possible ways of grasping M are enumerated andthe configurations ofM requiring a regrasp are identified.
The planner computes the path �obj so that M avoidscollision with the static obstacles Bj . This is done usingRPP (Randomized Path Planner), which is thus a componentof our planner. RPP is described in detail in [3, 16].
RPP generates �obj as a list of adjacent configurationsin a fine grid placed over Cobj (the 6D C-space of M), byinserting one configuration after the other starting with theinitial configuration of M. The original RPP only checksthat each inserted configuration is collision-free. To en-sure that there exists a sequence of transfer paths movingM along �obj, we have modified RPP. The modified RPPalso verifies that at each inserted configuration, M can begrasped using a grasp from the input grasp set. This isdone in the following way. A grasp assignment at someconfiguration of M is a pair associating an element of thegrasp set defined for M and the identity of the graspingarm(s). Note that the same element of the grasp set mayyield different grasp assignments involving different arms.The planner enumerates all the grasp assignments at the ini-tial configuration ofM and keeps a list of those which canbe achieved without collision between the grasping arm(s)and the obstacles, and between the grasping arms shouldthere be more than one. We momentarily ignore the pos-sibility that the grasping arm(s) may collide with the othernon-grasping arms. The list of possible grasp assignmentsis associated with the initial configuration. Prior to insert-ing any new configuration in the path being generated, RPPprunes the list of grasp assignments attached to the previ-ous configurationby removing all those which are no longerpossible at the new configuration. The remaining sublist,if not empty, is associated with this configuration which isappended to the current path.
If during a down motion of RPP (a motion along thenegated gradient of the potential field used by RPP) thelist of grasp assignments pruned as above vanishes at all
the successors of the current configuration (call it qobj),the modified RPP resets the list attached to qobj to containall the possible grasp assignments at qobj (as we proceedfrom the initial configuration). During a random motion (amotion intended to escape a local minimum of the potential),the list of grasp assignments is pruned but is constrainedto never vanish. In the process of constructing �obj , themodified RPP may reset the grasp assignment list severaltimes.
If successful, the outcome of RPP is a path �obj describedas a series of configurations of M, each annotated with agrasp assignment list. The path �obj is thus partitioned intoa series of subpaths, each connecting two successive con-figurations. It defines as many transfer tasks as there aredistinct grasp assignments associated with it. By construc-tion, for each such transfer task, there exists a transfer pathsatisfying the corresponding grasp assignment. The num-ber of regrasps along the generated path �obj is minimal,but RPP does not guarantee that this is the best path in thatrespect.
Details and comments: The condition that the samegrasp assignment be possible at two neighboring configu-rations of M does not guarantee that the displacement ofM can be done by a short (hence, collision-free) motionof the grasping arm(s). An additional test is needed whenthe set of grasps between two consecutive configurations ispruned. In our implementation, we assume that each armhas an inverse kinematics solution. Thus, an arm can attaina grasp with a finite set of different postures, determined byusing the arm’s inverse kinematics. We include the postureof each involved arm in the description of a grasp assign-ment. Hence the same combination of arms achieving thesame grasp, but with two different postures of at least onearm, defines two distinct grasp assignments. Then a con-figuration of M, along with a grasp assignment, uniquelydefines the configurations of the grasping arms. The res-olution of the grid placed across Cobj is set fine enoughto guarantee that the motion between any two neighboringconfigurations ofM results in a maximal arm displacementsmaller than some prespecified threshold.
In addition, we make considerations for sliding grasps.Indeed, for some manipulation tasks, the object must beallowed to slide in the grasp of the arms to achieve the goal.For the grasp assignments in the grasp list, we identify theirfeasible neighbors and add them to the list. A neighbor-ing grasp assignment is one where the grasp location andorientation is within some threshold distance from the orig-inal grasp assignment, and the same arm(s) and posture(s)is utilized. We choose a threshold distance that is smallenough to ensure that it is feasible to slide between neigh-boring grasp assignments. By updating the grasp list in thismanner, directions where a sliding grasp is necessary canthen be explored during the search for an object path.
A transfer path could be obstructed by the arms not cur-rently grasping M. Dealing with these arms can be par-ticularly complicated. In our current implementation, weassume that each arm has a relatively non-obstructive con-figuration given in the problem definition (in the systemshown in Fig. 1, the given non-obstructive configuration ofeach arm is when they are held out to the side). Prior toa transfer path, all arms not involved in grasping M aremoved to their non-obstructive configurations. The plan-ner nevertheless checks that no collision occurs with themduring the construction of �obj .
Perhaps the most blatant limitationof our approach is thatit does not plan for regrasps at configurations ofMwhere itmakes contact with obstacles (as we said, �obj is computedfree of collisions with obstacles). Since the object cannotlevitate, we require that M be held at all times duringregrasp. We assume that if M requires more than onearm to move, any subset of a grasp is sufficient to achievestatic stability during the regrasp. For example, if a grasprequires two arms, any one of these arms, alone, achievesstatic stability, allowing the other arm to move along atransit path. An obvious example where this limitation mayprevent our planner from finding a path is when the systemcontains a single arm; no regrasp is then possible.
RPP is only probabilistically complete [3]. If a path existsfor M, it will find it, but the computation time cannot bebounded in advance. Furthermore, if no path exists, RPPmay run forever. Nevertheless, for a rigid object (as is thecase for M), RPP is usually very quick to return a path,when one exists. Hence, we can easily set a time limitbeyond which it is safe to assume that no path exists.
RPP requires a postprocessing step to smooth the jerkyportions of the path, due to the random walks. Other pathplanners could possibly be used in place of RPP.
Generation of transit paths: The transfer tasks iden-tified as above can be organized into successive layers, asillustrated in Fig. 3. Each layer contains all the transfer tasksgenerated for the same subpath of �obj ; the transfer tasksdiffer by the grasp assignment. Selecting one such task inevery layer yields a series of transit tasks: the first consistsof achieving the first grasp from the initial system config-uration; it is followed by a possibly empty series of transittasks to change grasps between two consecutive transfertasks; the last transit task is to achieve the goal system con-figuration. Hence, it remains to identify a grasp assignmentin each layer of the graph shown in Fig. 3, such that thereexist transit paths accomplishing the corresponding transittasks.
qisys
qsysg
Nodes corresponding to SubPath A
Nodes corresponding to SubPath B
Root Node
Goal Node
A link can be constructed between nodes
Transfer Task
2
Transfer Task
1A A
Transfer Task
2
Transfer Task
1B B
Figure 3. The directed and layered graph.
Assume without loss of generality that all arms are ini-tially at their non-obstructive configurations. Our plannerfirst chooses an arbitrary transfer task in the first layer.Consider the transit task of going from the initial systemconfiguration to the configuration where the arms achievethe grasp assignment specified in the chosen transfer task,withM being at its initial configuration. The coordinatedpath of the arms is generated using RPP. If this fails, a newattempt is made with another transfer task in the first layer;otherwise, a transfer task is selected in the second layer.The connection of the system configuration at the end ofthe first transfer task to the system configuration at the startof this second transfer task forms a new transit task.
The transit task between two transfer tasks is more dif-ficult to solve. Actually, the difficulty arises when an un-grasp/regrasp is required. If the initial and goal grasp as-signments are neighbors, then we simply slide the grasp.To understand the difficulty of the case where an un-grasp/regrasp is required, imagine the situation where Mis a long bar requiring two arms to move. Assume thatthe system contains only two arms and that the bar canbe grasped at its two ends and at its center. Consider thesituation where the bar is grasped at its two ends and theregrasp requires swapping the two arms. This regrasp is notpossible without introducing an intermediate grasp. Forexample: arm 1 will ungrasp one end of the bar and regraspit at its center (during this regrasp, arm 2 will be holdingthe bar without moving); then arm 2 will ungrasp the barand regrasp it at the other end; finally, arm 1 will ungraspthe center of the bar and will regrasp it at its free end.
We address this difficulty by breaking the transit task be-tween two transfer tasks into smaller transit subtasks. Eachtransit subtask consists of going from one grasp assignmentto another in such a way that no two arms use the samegrasp at the same time. In this process, we allow arms notinvolved in the first and last assignment to be used. Westart with the first grasp assignment and generate all of thepotential grasp assignments that may be achieved from it
(assuming the corresponding transit paths exist). We gen-erate the successors of these new assignments, and so onuntil we reach the desired assignment (the one used in thenext transfer task). For each sequence that achieves thisdesired assignment, we test that it is actually feasible byusing RPP to generate a transit path between every two suc-cessive grasp assignments. We stop as soon as we obtain afeasible sequence. The concatenation of the correspondingsequence of transit paths forms the transit path connectingthe two considered transfer tasks. We then proceed to linkto the next layer of transfer tasks.
When we reach a transfer path in the last layer, its con-nection to the goal system configuration is carried out in thesame way as the connection of the initial system configura-tion to the first layer.
The result is an alternating sequence of transit and trans-fer paths that connects the initial configuration q i
sys to thegoal configuration qg
sys.
4 Human-Arm Kinematics
We now outline the method by which we determine the armposture for a human arm given the position and orientationof its hand. We present the algorithm using the right armfor illustration purposes. We assume that the torso and theshoulder positions are given.
The algorithm is based on two results from neurophysiol-ogy. The first result has to do with decoupling the probleminto two more manageable subproblems. Lacquaniti andSoechting have shown that the arm and wrist posture arefor the most part independent of each other [14]. Thisallows us to decouple the problem into finding first, theforearm and upper arm posture to match the hand position,and then determining the joint angles for the wrist to matchthe hand orientation. This is exactly the approach taken insolving the inverse kinematics of a robot manipulator withsix degrees of freedom and whose wrist joints intersect at apoint [23].
It is also known in neurophysiology that the arm posturefor pointing is mainly determined by a simple sensorimotortransformation model. Soechting and Flanders [29, 30]conducted experiments where the test subject was instructedto move the end of a pen-sized stylus to various targets intheir vicinity. From this study, they have devised a modelthat determines the posture of the forearm and upper armgiven the position of the end of the stylus. We use thismodel to determine the shoulder and elbow joint anglesgiven the position of the hand. Then, determining the wristjoint angles is a simple additional step.
4.1 Arm posture
Using the sensorimotor transformation model of Soechtingand Flanders [29, 30] we determine the arm posture giventhe location of the hand. To explain their model we firstdefine some generalized coordinates.
Denote the coordinate frame centered on the shoulder asthe shoulder frame. The x-axis is along the line that con-nects the two shoulders, the y-axis is parallel to the outwardnormal from the chest, and the z-axis points upwards to-wards the head. The parameters for the elevation and yawof the upper arm are � and �, respectively, and the parame-ters for the elevation and yaw of the forearm are � and �,respectively. The position of the wrist (or hand frame) isexpressed in terms of the spherical coordinates, azimuth �,elevation �, and radial distance R. The spherical coordi-nates are related to the cartesian coordinates of the shoulderframe by the equations
���
R2 � x2 � y2 � z2
tan� � x�y
tan� � z�p
x2 � y2�1�
These arm parameters are illustrated in Fig. 4.
�
�
�
�
R
� �
Shoulder
Elbow
Wrist
LateralAnterior
z
x
y
Figure 4. The parameters for the arm posture
The sensorimotor transformation model suggests that theparameters for arm posture are approximated by a linearmapping from the spherical coordinates of the hand frame(actually, Soechting and Flanders report this mapping fromthe position of the end of the stylus held by the test subject,but we simplify this to the hand). The relation is
�������
� � �40� 110R� 090�� � 394� 054R� 106�� � 132� 086�� 011�� � �100� 108�� 035�
�2�
where the units of measure are centimeters and degrees.Since this is only an approximation, plugging the arm pos-ture parameters back into the forward kinematics of thearm results in a positional error of the hand frame. This iscompensated for in the final stage of the inverse kinematicsalgorithm.
Once the generalized coordinates �, �, �, and � are ob-tained, they are transformed into the four joint angles ofthe forearm and the upper arm. We check to see if theyviolate any of their limits. If they are within their limits weproceed to find the wrist joint angles. In the event that alimit is violated (an illegal posture), an adjustment phase isinitiated.
For an illegal posture obtained from Eq. 2, it turns outthat the joint angle (the rotation around the upper arm asshown in Fig. 5) is the only one to violate its limits. Wecorrect for this in the following manner. Consider a newset of generalized coordinates of the arm, consisting of thewrist position and the angle � (the rotation of the elbowaround the axis between the shoulder and wrist as shownin Fig. 5). By decreasing the value of � from the initialillegal posture (moving the elbow upward), will moveback into the feasible joint range without ever changing thewrist position. Note that is obtained by transforming thewrist position and � into the joint angles of the arm.
�
Shoulder
Elbow
Wrist
LateralAnterior
Horizontal reference vectory
x
z
Figure 5. The elbow and shoulder rotation.
To summarize, the arm posture is estimated in the fol-lowing way:
1. The arm posture is obtained using the transformationof Eq. 2.
2. The values for and � are calculated.
3. If violates its joint limit, then � is decreased until thecorresponding satisfies its joint limit.
4.2 Wrist joints
The next step is to obtain the joint angles for the wrist.We assume that the three wrist joint angles intersect at
a point, consequently their values can be determined usingbasic inverse kinematics techniques. If the resulting wristangles violate their limits, we adjust the posture of the armuntil a feasible joint value is obtained. As before, this isachieved by incremently changing the angle � (rotation ofthe elbow) until either a feasible solution is obtained or wehave exhausted the possible values for �. In the case wherethis step fails, we take the closest feasible answer to thedesired hand orientation and proceed to the final adjustmentstep.
4.3 Final adjustment
The final step is to adjust the joint angles such that theexact position and orientationof the hand is obtained (recallthat the sensorimotor transformation model leaves the handposition slightly off from the desired location). Let e be the6 � 1 vector containing the position and orientation errorof the hand frame, and q be the 7 � 1 vector of arm jointangles. The relation between these two vectors is
e � J�q�q �3�
where J�q� is the 6 � 7 Jacobian matrix. By solving for qwe can iteratively change the joint angles to zero the error.
This simultaneous equation has only six linear con-straints, thus resulting in an infinite number of solutions.We remedy this situation by utilizing a solution of Eq. 3expressed as
q � q0 � r n �4�
where q0, r, and n are respectively, an instance of q, a scalarparameter, and the null space of Eq. 3. In this case, the nullspace is one dimensional and is expressed by the multipli-cation of the scalar r and its basis vector n. We employ aconstrained optimization to select the q which minimizes itsnorm while satisfying the arm joint limits. The joint anglesare then incrementally adjusted accordingly. This ensuresthat the modification to the hand position and orientationoccurs with minimal joint movement. This procedure isiteratively applied until the desired position and orientationof the hand is obtained. The resulting joint angles com-prise the posture of the arm for the given hand position andorientation.
The algorithm does not return an answer if the hand frameis out of the workspace of the arms, or if the method failsto correct for a set of joint angles which violates its limits.
5 Natural Motion
The multi-arm manipulation planner works regardless ofwhether the arms are human or robotic. In its most ba-sic state, there is no consideration of producing realisticmotions for the arms. Only kinematic constraints are rec-ognized, for example: requiring the motion to be collisionfree and allowing the arms to grasp the movable object atall times.
However, for the purpose of computer animation the mo-tions must appear natural. For robot arms, since they areundeniably artificial, there is no notion of what comprisesnatural movement. Though we may need to time parame-terize their path to yield realistic animation (i.e. reasonableacceleration and decceleration of the arms) there is no needto impose a naturalness constraint on the planner. In con-trast, for human arms there is clearly a natural manner inwhich they move. Fortunately, neurophysiology gives us areference to which we can compare our methods and resultswith experimental data on human motions. In this section,we discuss how our planner with the human-arm inversekinematics satisfies applicable naturalness constraints onederives from neurophysiology.
Dynamics versus Kinematics: We make no consider-ation of dynamics in our planning. From the neurophysio-logical viewpoint this is not a problem. It is widely agreedupon that arm movement is represented kinematically [26].It is then a postprocessing step, where the dynamics or mus-cle activation patterns are determined. With our method,by applying the appropriate time parameterization to theplanned path [8, 1] one can emulate results found in neuro-physiology. However, this is not done in our current planneryet.
Inverse Kinematics: The sensorimotor transformationmodel is derived from static arm postures. To verify that ourinverse kinematics algorithm is applicable to manipulationmotions, we consider the experimental results of Soechtingand Terzuolo [27]. Their experiment is for curvilinear wristmotions in an arbitrary plane. They find that humans exhibitthe following behavior:
1. The modulation in the elevation and yaw angles of theupper arm and forearm (�, �, �, and �) are close tosinusoidal.
2. The phase difference between � and � is 180�, at leastfor elevation angles of the plane between 0� and 45�.
3. The phase difference between � and � is close to180� � �, where � is the slant parameter describingthe curvilinear wrist motion.
We reproduced their experiment by tracing curvilinear wristmotions and generating the arm parameters using the humanarm inverse kinematics algorithm. Our finding is that the
computed and experimental values match quite nicely. Werefer the reader to [13] for a detailed explanation. Further-more, Soechting and Terzuolo find that for learned trajec-tories, their results for curvilinear wrist motions in a planehold true in three-dimensional space [28]. We justify theuse of our inverse kinematics algorithm for manipulationplanning based on these experimental results. Note, we donot claim that the posture found by our algorithm is the onlyone that is natural. Clearly, there are other postures that hu-mans assume depending upon the situation. For example,the redundant degree of freedom could be used to posturethe arm such that it avoids obstacles. By having a libraryof different inverse kinematics algorithms for human arms,the planner could consider a variety of natural postures fora given hand position and orientation.
Point to Point Arm Motion: When the arms move tograsp or regrasp an object, the planning is done in the jointspace of the arms. Since RPP is utilized to find the path,postprocessing is required to transform the collision freebut jerky motion into a smooth one (the jerky motion is dueto the random walks utilized by RPP). This smoothing isachieved by attempting to shorten the path by interpolatingbetween its points. The result is essentially a piece-wisejoint interpolated path. Hollerbach and Atkeson speculatethat the underlying planning strategy for such arm motionsis a staggered joint interpolation [10]. Staggered joint inter-polation is a generalization of joint interpolation introducedby Hollerbach and Atkeson to fit a greater range of exper-imental data. We have yet to implement a staggered jointinterpolation scheme for the “smoother”, but at present weare content with the arm motionsproduced thus far (i.e. jointinterpolation appears to be a close enough approximation).
We were unable to find any studies on human regrasping.Thus, no comment can be made on how reasonable theplanning strategy is from this viewpoint.
6 Experimental Results
We implemented the above approach in a planner written inC and running on a DEC Alpha workstation under UNIX.Experiments were conducted with a seated human figureand a robot arm. Each human arm has seven degrees offreedom, plus an additional nineteen degrees of freedomfor each hand. The non-obstructive configurations for thehuman arms are ones in which they are held out to theside. The robot has six revolute joints and three degrees offreedom for the end-effector. The non-obstructive configu-ration for the robot is one in which it stands vertically. Inaddition, we seat the human on a swivel chair. By addingthis extra degree of freedom, we allow the arms to accessa greater region, and hence tackle more interesting manip-ulation tasks. The rotation of the chair tracks the object to
keep it, essentially, in an optimal position with respect tothe workspace of the arms [20].
Fig. 1 shows a path generated by the planner for thehuman arms to bring the glasses on the table to the headof the human figure. We specify that both arms should beused to manipulate the glasses (this is defined in the graspset). Notice that during the regrasping phase, at least onearm is holding the glasses at all times. For this path it tookone minute to identify the transfer tasks and an additionaltwo minutes to complete the manipulation path. For thegeneration of �obj , the object’s C-space was discretizedinto a 100� 100� 100 grid. For the generation of thetransit paths, the joint angles of the arms were discretizedinto intervals of 0.05 radians. The grasp set contained212 grasps, yielding grasp assignment lists with up to 424elements.
Fig. 6 shows a path generated by the planner for thehuman arms and the robot arm cooperating to manipulatea chess box. Having the different arms working togetherpresents no difficulty to our planning approach. The plan-ner simply needs to know the correct inverse kinematicsalgorithm to apply to each arm. For this example, in defin-ing the grasp set, we specify two classes of grasps, one inwhich all three arms are used, and another in which onlythe two human arms are utilized. For this path it took aboutone and a half minutes to identify the transfer tasks and anadditional two minutes to complete the manipulation path.The same discretizations as above were used. The grasp setof the box contained 289 grasps yielding grasp assignmentlists with up to 2600 elements.
For manipulation planning with human arms, our currentimplementation is unable to plan motions where the armsare required to use their redundant degree of freedom toavoid obstacles. For example, a task where the arms mustplace an object deep into a tight box is almost impossiblefor our planner. The reason is simply that the sensorimotortransformationmodel does not consider obstacle avoidance.To tackle this class of problem, we will need to deviseanother inverse kinematics algorithm that does utilize theredundant degree of freedom for the purpose of avoidingobstacles. Again, some sort of naturalness constraint wouldneed to be satisfied.
For these examples, in computing the transit paths RPPuses the sum of the angular joint distances to the goal con-figuration as the guiding potential. Note that the motion ofthe fingers for the human and the robot are considered in thetransit paths (in moving from one grasp to another the fin-gers may change their posture). In computing�obj RPP usesan NF2-based potential with three control points [16]. Infinding both the transit paths and �obj , we limit the amountof computation spent in RPP to three backtrack operations[16], after which the planner returns failure. Failure to find�obj results in the immediate failure to find a manipulation
Figure 6. Planned path for human/robot arm cooperative manipulation.
path. Similarly, a failure to find transit paths to link to-gether the layers of transfer paths results in a failure to finda manipulation path. The time for the planner to report fail-ure depends on the problem, with some examples rangingfrom 30 seconds to a few minutes. The collision checkingalgorithm utilized is that of Quinlan [24].
7 Conclusion
We have presented a novel approach for solving the com-plicated multi-arm manipulation planning problem. Ourapproach embeds several simplifications yielding an imple-mented planner that is not fully general. However, exper-iments with this planner show that it is quite reliable andefficient in finding manipulation paths, when such pathsexist, making it suitable as part of an interactive tool tofacilitate the animation of scenes. We believe the robustnature of the planner is the result of careful considerationof the general manipulation problem, the introduction ofreasonable simplifications, and the appropriate utilizationof the efficient randomized path planner.
We have also presented a new inverse kinematics algo-rithm for human arms based on neurophysiological studies.This algorithm, in conjunction with the planner, automat-ically generates natural arm motions for a human figuremanipulating an object.
In the future we hope to include the ability to regraspthe movable object by having the arms place it in a stableconfiguration against some obstacles. We also plan to useexisting results to automatically compute the grasp set of anobject from its geometric model. The technique describedin [11] to deal with multiple movable objects in a 2D spaceshould also be applicable to our planner. Furthermore,we hope to time parameterize the motion paths to yieldrealistic velocities, by implementing one of the many suchalgorithms for robot and human arms [8, 1, 4]. Finally,we hope to explore and devise other inverse kinematicsalgorithms for the arms, as well as incorporating twistingand bending of the torso. Ultimately, we aim to create atask-level animation package for human motions.
Acknowledgments
Y. Koga is supported in part by a Canadian NSERC fellow-ship. The use of Sean Quinlan’scollision checking softwareis gratefully acknowledged.
References
[1] Atkeson, Christopher and Hollerbach, John. Kine-matic Features of Unrestrained Arm Movements. MITAI Memo 790, 1984.
[2] Baraff, David. Analytical Method for Dynamic Simu-lation of Non-Penetrating Rigid Bodies. Proceedingsof SIGGRAPH’89 (Boston, Massachusetts, July 31 -August 4, 1989). In Computer Graphics 23, 3 (July1989), 223-232.
[3] Barraquand, Jerome and Latombe, Jean-Claude.Robot Motion Planning: A Distributed Representa-tion Approach. Int. J. Robotics Research, 10(6), De-cember 1991, 628-649.
[4] Bobrow, J.E., Dubowsky, S., Gibson, J.S. Time-Optimal Control of Robotic Manipulators AlongSpecified Paths. Int. J. Robotics Research, Vol. 4, No.3, 1985, 3-17.
[5] Bruderlin, Armin and Calvert, Thomas. Goal-directed,dynamic animation of human walking. Proceedingsof SIGGRAPH’89 (Boston, Massachusetts, July 31 -August 4, 1989). In Computer Graphics 23, 3 (July1989), 233-242.
[6] Ching, Wallace and Badler, Norman. Fast motionplanning for anthropometric figures with many de-grees of freedom. Proc. 1992 IEEE Int. Conf. onRobotics and Automation, Nice, France, 1992, 2340-2345.
[7] Ferbach, Pierre and Barraquand, Jerome. A PenaltyFunction Method for Constrained Motion Planning.Rep. No. 34, Paris Research Lab., DEC, Sept. 1993.
[8] Flash, T. and Hogan, N. The Coordination of ArmMovements: An Experimentally Confirmed Mathe-matical Model. MIT AI Memo 786, 1984.
[9] Hahn, James K. Realistic Animation of Rigid Bod-ies. Proceedings of SIGGRAPH’88 (Atlanta, Geor-gia, August 1-5, 1988). In Computer Graphics 22, 4(August 1988), 299-308.
[10] Hollerbach, John and Atkeson, Christopher. DeducingPlanning Variables from Experimental Arm Trajecto-ries: Pitfalls and Possibilities. BiologicalCybernetics,56(5), 1987, 279-292.
[11] Koga, Y., Lastennet, T., Latombe, J.C., andLi, T.Y. Multi-Arm Manipulation Planning. Proc. 9thInt. Symp. Automation and Robotics in Construction,Tokyo, June 1992, 281-288.
[12] Koga, Yoshihito. On Computing Multi-Arm Manipu-lation Trajectories. Ph.D. thesis, Stanford University(in preparation).
[13] Kondo, Koichi. Inverse Kinematics of a Human Arm.Rep. STAN-CS-TR-94-1508, Department of Com-puter Science, Stanford University, CA, 1994.
[14] Lacquaniti, F. and Soechting, J.F. Coordination of Armand Wrist Motion During A Reaching Task. The Jour-nal of Neuroscience, Vol. 2, No. 2, 1982, 399-408.
[15] Laumond, Jean-Paul and Alami, Rachid. A Geomet-rical Approach to Planning Manipulation Tasks: TheCase of a Circular Robot and a Movable CircularObject Amidst Polygonal Obstacles. Rep. No. 88314,LAAS, Toulouse, 1989.
[16] Latombe, Jean-Claude. Robot Motion Planning.Kluwer Academic Publishers, Boston, MA, 1991.
[17] Lee, P., Wei, S., Zhao, J., and Badler, N.I. Strengthguided motion. Proceedings of SIGGRAPH’90 (Dal-las, Texas, August 6-10, 1990). In Computer Graphics24, 4 (August 1990), 253-262.
[18] Lengyel, J., Reichert, M., Donald, B.R., and Green-berg, D.P. Real-Time Robot Motion Planning UsingRasterizing Computer Graphics Hardware. Proceed-ings of SIGGRAPH’90 (Dallas, Texas, August 6-10,1990). In Computer Graphics 24, 4 (August 1990),327-335.
[19] Lozano-Perez, Tomas. Spatial Planning: A Configu-ration Space Approach. IEEE Tr. Computers, 32(2),1983, pp. 108-120.
[20] McCormick, E.J. and Sanders, M.S. Human Factorsin Engineering and Design. McGraw-Hill Book Com-pany, New York, 1982.
[21] McKenna, Michael and Zeltzer, David. Dynamic sim-ulation of autonomous legged locomotion. Proceed-ings of SIGGRAPH’90 (Dallas, Texas, August 6-10,1990). In Computer Graphics 24, 4 (August 1990),29-38.
[22] Pertin-Troccaz, Jocelyn. Grasping: A State of the Art.In The Robotics Review 1, O. Khatib, J.J. Craig, andT. Lozano-Perez, eds., MIT Press, Cambridge, MA,1989, 71-98.
[23] Pieper, D. and Roth, B. The Kinematics of Manip-ulators Under Computer Control. Proceedings of theSecond International Congress on Theory of Machinesand Mechanisms, Vol. 2, Zakopane, Poland, 1969,159-169.
[24] Quinlan, Sean. Efficient Distance Computation Be-tween Non-Convex Objects. To appear in Proc. 1994IEEE Int. Conf. on Robotics and Automation, SanDiego, CA, 1994.
[25] Raibert, Marc and Hodgins, Jessica. Animation ofDynamic Legged Locomotion. Proceedings of SIG-GRAPH’91 (Las Vegas, Nevada, July 28 - August
2, 1991). In Computer Graphics 25, 4 (July 1991),349-358.
[26] Smith, A.M. et al. Group Report: What Do Studies ofSpecific Motor Acts Such as Reaching and GraspingTell Us about the General Principles of Goal-DirectedMotor Behaviour? Motor Contro: Concepts and Is-sues, D.R Humphrey and H,J, Freund, eds., John Wi-ley and Sons, New York, 1991, 357-381.
[27] Soechting, J.F. and Terzuolo, C.A. An Algorithm forthe Generation of Curvilinear Wrist Motion in anArbitrary Plane in Three Dimensional Space. Neu-roscience, Vol. 19, No. 4, 1986, 1393-1405.
[28] Soechting, J.F. and Terzuolo, C.A. Organization ofArm Movements in Three Dimensional Space. WristMotion is Piecewise Planar. Neuroscience, Vol. 23,No. 1, 1987, 53-61.
[29] Soechting, J.F. and Flanders, M. Sensorimotor Rep-resentations for Pointing to Targets in Three Dimen-sional Space. Journal of Neurophysiology, Vol. 62,No. 2, 1989, 582-594.
[30] Soechting, J.F. and Flanders, M. Errors in Pointingare Due to Approximations in Sensorimotor Transfor-mations. Journal of Neurophysiology, Vol. 62, No. 2,1989, 595-608.
[31] Soechting, J.F. Elements of Coordinated Arm Move-ments in Three-Dimensional Space. Perspectives onthe Coordination of Movement, edited by S.A. Wal-lace, Elsevier Science Publishers, Amsterdam, 1989,47-83.
[32] Tournassoud, P., Lozano-Perez, T., and Mazer, E. Re-grasping. Proc. IEEE Int. Conf. Robotics and Automa-tion, Raleigh, NC, 1987, 1924-1928.
[33] Webber, B., Badler, N., Baldwin, F.B., Beckett, W.,DiEugenio, B., Geib, C., Jung, M., Levinson, L.,Moore, M., and White, M. Doing what you’re told:following task instructions in changing, but hospitableenvironments. SIGGRAPH ’93 Course note 80 “Re-cent Techniques in Human Modeling, Animation andRendering”, 1993, 4.3-4.31.
[34] Wilfong, G. Motion Planning in the Presence of Mov-able Obstacles. Proc. 4th ACM Symp. ComputationalGeometry, 1988, 279-288.
