Belajar Dynamic 2

UNIVERSITY OF CALIFORNIA
IRVINE
Dynamic Motion Planning For Robot
Manipulators Using B-Splines
DISSERTATION
submitted in partial satisfaction of the requirements for the degree of
DOCTOR OF PHILOSOPHY
in Engineering
by
Chia-Yu Eric Wang
Dissertation Committee:
Professor James E. Bobrow, Chair
Professor Athanasios Sideris
Professor David J. Reinkensmeyer
2001
c _2001 by Chia-Yu Eric Wang
The dissertation of Chia-Yu Eric Wang is approved,
and is acceptable in quality and form
for publication on microlm:
Committee Chair
University of California, Irvine
2001
ii
In Memory Of My Best Friend,
Melissa Wang.
iii
Contents
List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii
List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xii
Acknowledgements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii
Curriculum Vitae . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xv
Abstract of the Dissertation . . . . . . . . . . . . . . . . . . . . . . . . . . xvii
Chapter 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2.1 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2.2 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2.3 System Identication . . . . . . . . . . . . . . . . . . . . . . . 4
1.2.4 Dynamic Motion Planning for Robots . . . . . . . . . . . . . . 4
1.2.5 Robotic Body Weight Supported (BWS) Training . . . . . . . 6
1.3 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.4 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
Chapter 2 Kinematics of Robot Manipulators . . . . . . . . . . . . 11
2.1 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.1.1 Lie Groups and Lie Algebras . . . . . . . . . . . . . . . . . . . 12
2.1.2 Adjoint Operators . . . . . . . . . . . . . . . . . . . . . . . . 15
2.1.3 Forward Kinematics . . . . . . . . . . . . . . . . . . . . . . . 17
2.1.4 Derivatives of the Adjoint Mapping . . . . . . . . . . . . . . . 18
2.2 Dierential Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.2.1 The Manipulator Jacobian . . . . . . . . . . . . . . . . . . . . 20
2.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
Chapter 3 Dynamics of Robot Manipulators . . . . . . . . . . . . . 25
3.1 Open-Chained Systems . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.1.1 Inverse Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.1.2 Forward Dynamics . . . . . . . . . . . . . . . . . . . . . . . . 28
iv
3.1.3 Hybrid Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.2 Closed-Chained Systems . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.2.1 Lathrops Algorithm . . . . . . . . . . . . . . . . . . . . . . . 35
3.3 Derivatives of Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 37
3.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
Chapter 4 B-Spline Curves . . . . . . . . . . . . . . . . . . . . . . . . 40
4.1 Construction of B-Splines . . . . . . . . . . . . . . . . . . . . . . . . 40
4.1.1 B-Spline Basis Functions . . . . . . . . . . . . . . . . . . . . . 42
4.2 Examples of B-Splines . . . . . . . . . . . . . . . . . . . . . . . . . . 45
4.3 Derivatives of B-Splines . . . . . . . . . . . . . . . . . . . . . . . . . 47
4.4 Uniform Quintic B-Spline Curves . . . . . . . . . . . . . . . . . . . . 49
Chapter 5 Dynamic Motion Planning for Open-Chained Robots . 53
5.1 Optimization Problems for Robots . . . . . . . . . . . . . . . . . . . 54
5.2 Identication of Mass Properties . . . . . . . . . . . . . . . . . . . . . 56
5.2.1 Application: PUMA 762 . . . . . . . . . . . . . . . . . . . . . 59
5.3 Weightlifting Motion Planning . . . . . . . . . . . . . . . . . . . . . . 62
5.3.1 Optimization Results . . . . . . . . . . . . . . . . . . . . . . . 65
5.3.2 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
5.3.3 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
Chapter 6 Dynamic Motion Planning for the Design of Robotic Gait
Rehabilitation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
6.2 Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
6.2.1 Human Model and Walking Motion . . . . . . . . . . . . . . . 77
6.2.2 Formulation of the Optimal Control Problem . . . . . . . . . . 81
6.2.3 Dynamic Motion Optimization via Direct Parameter Optimization 88
6.3 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
6.3.1 Case 1: Paralyzed Swing Leg with Motion Captured Stance Hip
Orientation (No Optimization) . . . . . . . . . . . . . . . . . 91
6.3.2 Case 2: Unimpaired Swing Leg with Eort Minimization of All
Joints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
6.3.3 Case 3: Paralyzed Swing Leg with Eort Minimization of the
Stance Hip Torques . . . . . . . . . . . . . . . . . . . . . . . . 96
6.3.4 Case 4: Paralyzed Swing Leg with Eort Minimization of the
Stance Hip Torques and Bounded Stance Hip Orientation . . . 97
6.3.5 Comparison of Cases . . . . . . . . . . . . . . . . . . . . . . . 100
6.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
Chapter 7 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
7.1 Research Summary and Conclusions . . . . . . . . . . . . . . . . . . . 108
7.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109
v
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
Appendix A Transformation Between Joints and Motion Capture
Markers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
Appendix B Estimation of Dynamical Properties Using Regression
Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121
Appendix C Results for Gait Rehabilitation Case 1: Paralyzed
Swing Leg with Motion Captured Stance Hip Orientation . . . . . . . 122
Appendix D Results for Gait Rehabilitation Case 2: Unimpaired
Swing Leg with Eort Minimization of All Joints . . . . . . . . . . . . 126
Appendix E Results for Gait Rehabilitation Case 3: Paralyzed
Swing Leg with Eort Minimization of the Stance Hip Torques . . . 136
Appendix F Results for Gait Rehabilitation Case 4: Paralyzed
Swing Leg with Eort Minimization of the Stance Hip Torques and
Bounded Stance Hip Orientation . . . . . . . . . . . . . . . . . . . . . . . 146
vi
List of Figures
1.1 Body weight supported (BWS) step training following spinal cord in-
jury. Two therapists assist in leg movement, while a third assists in
torso movement. From Behrman and Harkema (2000). . . . . . . . . 7
4.1 A cubic B-spline curve (solid line) constructed on the basis functions
(dashed lines). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
4.2 Local control of the B-spline curves. . . . . . . . . . . . . . . . . . . . 41
4.3 A quadratic B-spline produced from the recurrence. . . . . . . . . . . 44
4.4 A step-function B-spline curve with the control points 1.5, 1, 2, 1
over the knots 2, 4, 5, 7.5, 9. . . . . . . . . . . . . . . . . . . . . . . 45
4.5 A piecewise linear B-spline curve with the control points 2, 1, 3, 0, 1.5
over the knots 1, 2.5, 3.5, 6, 6, 7.5, 10. . . . . . . . . . . . . . . . . . 47
5.1 The home position of the PUMA with all the joint positions = 0. . . 59
5.2 The initial (left) and nal (right) congurations of the puma. . . . . . 66
5.3 The motion, speed and torque of the 4
th
joint in the 1 DOF case.
(Limits: dotted lines) . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
5.4 The motion of the wrist in the 3 DOF case. . . . . . . . . . . . . . . 69
5.5 The motion for the 6 DOF case. . . . . . . . . . . . . . . . . . . . . . 70
6.1 Body weight supported (BWS) step training following spinal cord in-
jury. Two therapists assist in leg movement, while a third assists in
torso movement. From Behrman and Harkema (2000). . . . . . . . . 75
6.2 One-half of the gait cycle (duration 0.467 sec). . . . . . . . . . . . . . 77
6.3 Muscle system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
6.6 One-half of the gait cycle (duration 0.5 sec). . . . . . . . . . . . . . . 84
6.8 The resulting gait (duration 0.5 sec) for Case 1: Paralyzed Swing Leg
with Motion Captured Stance Hip Orientation (No Optimization). The
solid lines show the resulting gait and the dashed lines are the gait
recorded from the motion capture system.) . . . . . . . . . . . . . . . 91
6.9 The resulting gait (duration 0.5 sec) for Case 2: Unimpaired Swing
Leg. The solid lines show the resulting gait and the dashed lines are
the gait recorded from the motion capture system.) . . . . . . . . . . 93
vii
6.10 The resulting joint motions (duration 0.5 sec) for Case 2: Unimpaired
Swing Leg. The solid lines show the resulting joint motions and the
dashed lines are the joint motions recorded from the motion capture
system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
6.11 The resulting joint torques (duration 0.5 sec) for Case 2: Unimpaired
Swing Leg. The solid lines show the resulting joint torques and the
dashed lines are the joint torques corresponding to the motion capture
data.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95
6.12 The resulting gait (duration 0.5 sec) for Case 3: Paralyzed Swing Leg.
The solid lines show the resulting gait and the dashed lines are the gait
6.13 The resulting joint motions (duration 0.5 sec) for Case 3: Paralyzed
system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
6.14 The resulting joint torques (duration 0.5 sec) for Case 3: Paralyzed
data.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
6.15 The resulting gait (duration 0.5 sec) for Case 4: Paralyzed Swing Leg
with Bounded Stance Hip Orientation. The solid lines show the result-
ing gait and the dashed lines are the gait recorded from the motion
capture system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
6.16 The resulting joint motions (duration 0.5 sec) for Case 4: Paralyzed
Swing Leg with Bounded Stance Hip Orientation. The solid lines show
the resulting joint motions and the dashed lines are the joint motions
6.17 The resulting joint torques (duration 0.5 sec) for Case 4: Paralyzed
the resulting joint torques and the dashed lines are the joint torques
corresponding to the motion capture data.) . . . . . . . . . . . . . . . 103
6.18 Applied eort for step duration 0.5 sec. (The , , and
marks represent Case 1, 2, 3 and 4, respectively.) . . . . . . . . . . . 103
6.19 Root-Mean-Square of position error. (The , and marks
represent Case 2, 3 and 4, respectively.) . . . . . . . . . . . . . . . . . 104
6.20 Final position error. (The , and marks represent Case 2,
3 and 4, respectively.) . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
C.1 The resulting gait (duration 0.433 sec) for Case 1: Paralyzed Swing
Leg with Motion Captured Stance Hip Orientation (No Optimization).
viii
D.1 The resulting gait (duration 0.433 sec) for Case 2: Unimpaired Swing
D.4 The resulting joint motions (duration 0.433 sec) for Case 2: Unimpaired
system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130
D.5 The resulting joint torques (duration 0.433 sec) for Case 2: Unimpaired
data.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131
system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
data.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133
system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134
data.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135
ix
E.1 The resulting gait (duration 0.433 sec) for Case 3: Paralyzed Swing
E.4 The resulting joint motions (duration 0.433 sec) for Case 3: Paralyzed
system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140
E.5 The resulting joint torques (duration 0.433 sec) for Case 3: Paralyzed
data.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141
system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142
data.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143
system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144
data.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145
F.1 The resulting gait (duration 0.433 sec) for Case 4: Paralyzed Swing
Leg with Bounded Stance Hip Orientation. The solid lines show the re-
sulting gait and the dashed lines are the gait recorded from the motion
capture system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147
capture system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148
x
capture system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149
F.4 The resulting joint motions (duration 0.433 sec) for Case 4: Paralyzed
F.5 The resulting joint torques (duration 0.433 sec) for Case 4: Paralyzed
xi
List of Tables
5.1 The joint locations and associated rotation axes of the PUMA. . . . . 60
5.2 The physical properties without any prediction of mass and friction . 61
5.3 The physical properties with prediction of mass and friction . . . . . 62
5.4 The optimal results for three cases . . . . . . . . . . . . . . . . . . . 67
6.1 Link lengths . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
6.2 Dynamic properties of the human model (in the SI system of units) . 79
6.3 DOF of the human model. . . . . . . . . . . . . . . . . . . . . . . . . 85
6.4 Weighting coecients in Case 2: Unimpaired Swing Leg . . . . . . . . 92
6.5 Weighting coecients for swivel in Case 3: Paralyzed Swing Leg . . . 97
A.1 Joint width . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
xii
Acknowledgements
At the moment I prepared the writing of this Acknowledgments I went through
my memory from the time I started my Masters to the time I completed my Ph.D. A
smile was on my face because of the pleasure during my graduate studies at University
of California, Irvine. I am grateful to many of those people who made the time of my
study so enjoyable.
First of all, I would like to thank my advisor, Professor James Bobrow, for his
guidance of my Ph.D. work. His knowledge in the physical sense of mathematical
formulas led my research to concentrate on practical applications using theoretical
concepts. Implementing robots to move with our algorithms is appealing to us and
has been the main stimulation for this work. I also thank him for reviewing this
dissertation and providing numerous valuable recommendations.
Professor David Reinkensmeyer has dedicated his research in the eld of gait
rehabilitation. He was the person who came up with the idea of applying our algo-
rithms in the design of robots for gait rehabilitation. Much eort has been made and
many brilliant insights have been generated by him in this work. Without him this
work would not be done.
I would like to thank Professor Athanassios Sideris for serving on the committee
for my dissertation. I have taken several classes from him and his explicit instruction
helped me understand many fundamental concepts, especially in optimization. I
xiii
would also like to thank Professor Michael McCarthy for maintaining a unique and
wonderful environment for the people in the Robotics Lab and Professor William
Schmitendorf for introducing me to the study of research instead of the study of
course work.
Collaboration with the lab mates of the Robotics Lab, including Juanita Albro,
Butch Timoszyk, Craig Takahashi, Garett Sohl, Scott Ploen, Bryan Martin, Robert
Lienbach, Christian de Haro Carbonell, Eliot Shiosaki, Richard Srisamang, Tony
Nguyen, and David Pai, involved brainstorming and often resolved my confused
thoughts. I would like to express my thanks to Juanita Albro, Butch Timoszyk,
Craig Takahashi, Garett Sohl and Alba Perez for their friendships especially in the
last three years of my Ph.D.
Many friends of mine have made my life beyond school cheerful by organizing
activities, including tennis-playing, golng, swimming, hiking, and so on. Without
them, I would not be able to nish my Ph.D. in the rst year (2001) of the new
millennium but the last year (2000) of the old millennium. It seemed that I might
graduate a year earlier. (I was just kidding.) Among them, I would like to express
my appreciation to Jie Yu, Ben Hsieh, Ying Chang and Yizhang Zhou for being my
close friends and for all they have done for me.
Finally, I would like to thank my parents, my brothers, and Mei-Ling Tao for
their love and support. This work is dedicated to them.
xiv
Curriculum Vitae
Chia-Yu Eric Wang
1989 B.S. in Mechanical Engineering,
National Cheng Kung University, Taiwan
1995 M.S. in Mechanical and Aerospace Engineering,
2001 Ph.D. in Engineering,
FIELD OF STUDY
Robot Control
PUBLICATIONS
1. V. Coimbatore, C-Y. E. Wang, R. Ganeshan, J. Heckel, A. Baskin, and S.
C-Y. Lu, CITYWORK: A Collaboration Maintenance Management Application,
International CIRP Design Seminar, California, Oct. 1997
2. C-Y. E. Wang, W. K. Timoszyk, and J. E. Bobrow, Weightlifting Motion Planning
for a Puma 762 Robot, IEEE International Conference on Robotics and Automation,
Michigan, May 1999
xv
3. R. Ganeshan, F. Grobler, C-Y. E. Wang, and V. Coimbatore, CITYWORK:
Application of Collaborative Technologies for Infrastructure Management, Journal
of Computing in Civil Engineering, v15-1, pp.74-80, Jan. 2001
4. C-Y. E. Wang, W. K. Timoszyk, and J. E. Bobrow, Payload Maximization for
Open Chained Manipulators: Finding Weightlifting Motions for a Puma 762 Robot,
IEEE Transactions on Robotics and Automation, v17-2, pp.218-224, Apr. 2001
5. C-Y. E. Wang, J. E. Bobrow, and D. J. Reinkensmeyer, Swinging From The Hip:
Use of Dynamic Motion Optimization in the Design of Robotic Gait Rehabilitation,
IEEE International Conference on Robotics and Automation, Korea, May 2001
EXPERIENCE
1992-1993 Mechanical Engineer,
San Shing Hardware Works Co., Ltd., Taiwan
1993-1994 Project Leader,
Institute for Information Industry, Taiwan
1996-1997 Research Assistant,
Department of Mechanical Engineering,
University of Southern California
1994-2001 Research Assistant,
Department of Mechanical and Aerospace Engineering,
xvi
Abstract of the Dissertation
Dynamic Motion Planning For Robot Manipulators
Using B-Splines
by
Chia-Yu Eric Wang
Doctor of Philosophy in Engineering
University of California, Irvine, 2001
Professor James E. Bobrow, Chair
Although the dynamic equations of motion of open-chained robot systems are
well-known, they are seldom taken into account during the planning of motions. In
this work, we show that the dynamics of a robot can be taken into account in motion
planning. Two applications of dynamic motion planning are explored: 1) develop-
ment of point-to-point weightlifting motions for open-chained robots; and 2) design
of robotic gait rehabilitation. The governing optimal control problem is converted
into a direct, SQP parameter optimization in which the gradient is determined an-
alytically. The joint trajectories are dened by B-spline polynomials along with a
time-scale factor. In the rst application, a weightlifting motion planner is applied
to a Puma 762 robot, with its physical limitations incorporated into the formula-
tion. The problem is formulated as an optimal control problem for a fully actuated
xvii
articulated chain. The torque limits are formulated as soft constraints added into
the objective function while the position and velocity limits are formulated as hard,
linear inequality constraints, on the parameters. The solutions obtained with the mo-
tion planner extend the robots payload capability while reducing the joint torques.
Interestingly, nearly all the trajectories found pass through singular congurations,
where large internal forces from the robot are applied to the payload and little torque
is needed from the motors. In the second application, we examine a method to con-
trol the stepping motion of a paralyzed person suspended on a treadmill using a
robot attached to the pelvis. A leg swing motion is created by moving the pelvis
without contact with the legs. The problem is formulated as an optimal control
problem for an underactuated articulated chain. Motion capture data recorded from
an unimpaired human subject is compared to the simulation results from the dy-
namic motion optimization. Our results suggest that it is feasible to create a gait
for a paralyzed person that is close to that of an unimpaired subject by control-
ling the pelvis with a robot. The resulting motions of the two applications can be
found at the web sites http://www.eng.uci.edu/chwang/project/puma762.html
and http://www.eng.uci.edu/chwang/project/stepper/stepper.html.
xviii
Chapter 1
Introduction
In this dissertation, we refer to dynamic motion planning as the study of robot
motion planning with the dynamics of the robot taken into account. The majority of
current robot motion planning algorithms, such as those found in well-known texts
(see, e.g. [32]), ignore the dynamics of the robot during the motion planning phase.
Our study demonstrates how the dynamics can be used in motion planning to improve
the performance and eciency for robots and to design robots for gait rehabilitation.
1.1 Motivation
Robots have been used in automatic production lines for many decades. In
general, industrial robots are used to replace human workers in repetitive tasks. Most
industrial robots, such as the PUMA robot, have factory-established payload limits
which are small compared to their massive components. That draws our attention to
extending the payload capability of a robot to its maximum potential while taking
into account realistic constraints such as joint torque limits and velocity bounds. The
solution to the dynamic motion planning problem can also minimize joint torques for
a given load, thereby reducing wear on the robot.
1
As another important example of dynamic motion planning, we examine a
method to control the stepping motion of a paralyzed person suspended on a treadmill
using a robot attached to the pelvis. Clinical stepping motion training is currently
limited because the training is labor intensive. Multiple therapists are often required
to control the pelvis and legs. Implementing training with robotics is an attempt to
make the training less labor intensive, more consistent, and more widely accessible.
The objective of this study is to use dynamic motion planning to determine to what
extent repetitive stepping by a paralyzed patient can theoretically be generated by
assisting in pelvis motion alone.
1.2 Background
1.2.1 Kinematics
Kinematics of robot manipulators is the study of motion independent of the
forces which cause it. Any robot can be described kinematically by giving the values
of four quantities for each link. These quantities are called the Denavit-Hartenberg
parameters [21]. Brockett [13] connects the theory of Lie groups and robot kinematics
by introducing the product-of-exponential (POE) equations for the forward kinemat-
ics. Although the matrix exponential needs six independent parameters to specify a
joint twist, it is more straight-forward to construct the joint twist directly by writing
down the direction of the joint axis. One of the most attractive features of the POE
is its use of two coordinate frames, the base frame and the tool frame. This property,
combined with the geometric signicance of the twists, makes the POE representation
an attractive alternative to the use of Denavit-Hartenberg parameters.
2
1.2.2 Dynamics
Dynamics of robot manipulators is the study of the forces required to cause
motion. The problem of computing the forces required for a given motion is known
as the inverse dynamics problem. The Newton-Euler formulation is an ecient linear
recursive approach for inverse dynamics. The formulation is simplied by using a
spatial notation in Featherstone [23].
By reformulating the equations so that acceleration is computed as a function
of applied force, it becomes the forward dynamics. Perhaps the most familiar re-
cursive approach for open-chain forward dynamics is the articulated-body method
of Featherstone [23]. The concept of articulated-body inertias is also signicant,
and it is the central feature of Featherstones method. The algorithm computes the
articulated-body inertia of each set of links of the robot manipulators in a recursive
manner from the tip to the base. A second recursion, starting at the base and working
out to the tip, calculates the applied force or acceleration at each joint. Featherstones
algorithm has also been implemented using the spatial operator algebra of Rodriguez
et al. [51, 52].
Linear recursive approaches for forward dynamics of closed chains have been
presented by Lathrop [31]. In [31], general endpoint constraints are propagated from
the tip down to the base, where two 6 6 inverses are necessary to match the propa-
gated tip constraints to the existing base constraints. A second recursion from base to
tip solves for the joint accelerations. By modifying Lathrops method, we developed
a formulation for hybrid dynamics to compute either acceleration or applied force for
each joint.
3
Using the POE, Park and Bobrow [41] and Park et al. [42] derive a geometric
version of the Newton-Euler recursive inverse dynamics similar to Featherstones, but
based on existing geometric concepts of Lie groups. Lie groups can also be used
to develop the recursive approaches for forward dynamics in Ploen [45] and hybrid
dynamics of open chains and closed chains in this dissertation.
1.2.3 System Identication
In this dissertation, we develop a method for estimating the mass, the location
of center of mass, and the moments of inertia of each link and the friction at each joint
of a robot during general manipulator movement. These dynamic system parameters
are factorized linearly using the geometric operators of Lie groups and Lie algebras.
The Lie group formulation of inverse dynamics is re-arranged in a linear matrix form
of the dynamic properties. The least squares method with a QR-decomposition is
used to identify the dynamic properties after exciting the robot and collecting the
data of joint positions, velocities, accelerations and applied forces.
1.2.4 Dynamic Motion Planning for Robots
Dynamic motion planning is a motion planning problem in which the dynam-
ics of the robot is taken into account. We assume that the desired motion can
be characterized as the solution to a general optimal control problem of the form
min
(t)
J
c
= (q, q, t
f
) +
_
t
f
0
L(q, q, , t)dt subject to the dynamic equations of motion
and bounds on position, velocity and torque. The specic cost functionals used in
this dissertation will be given in Chapter 5 and 6. The nal time t
f
may be either
4
free or xed in our formulation. Because of the importance of nding solutions to
optimal control problems of the above form, there are several numerical algorithms
and commercial software packages available to solve them. All of the competitive
approaches incorporate some form of Newton iteration on a parameterized version of
the problem. Because the problems are nonconvex, the Newton iteration will con-
verge to a local minimizer. See [10] for a review on progress made in this area since
the 1960s.
Algorithms used to solve optimal control problems are broadly classied as either
indirect methods or direct methods. Indirect methods explicitly solve the optimality
conditions stated in terms of the maximum principle [14], the adjoint equations, and
the transversality (boundary) conditions. These conditions can be derived using the
calculus of variations, where the rst variation of the Hamiltonian function is set
to zero. A number of multiple shooting methods have been developed to solve the
two-point boundary value problem dened by the equations.
Although the indirect approach described above has been used successfully in
many applications, it has been replaced by direct methods in recent years. The
primary reasons are that the region of convergence is relatively small and that it is
dicult to incorporate path inequality constraints. We chose a direct approach for
the work reported in this dissertation. The basic idea is to parameterize the joint
trajectories, and to vary the parameters in a nonlinear optimization until a local
minimum which satises the constraints has been achieved. The joint trajectories
used in the motion planning problem can be represented in many ways. Among them
are C
2
splines and B-spline polynomials, which have been used by many robotics
researchers [15, 16, 1, 24, 35, 39, 44, 54]. B-spline polynomials provide local control
of the joint trajectory and they have the important convex hull property [6]. We
5
exploit the convex hull property by transforming the semi-innite constraints that
arise in the problem formulation into nite dimensional ones. In order to vary the
total time taken for the motion, we have added a time-scale factor. This allows us
to solve a free-nal-time problem in which the derivatives of the joint positions with
respect to the time-scale factor can be derived analytically.
By parameterizing the joint trajectories, the optimal control problem becomes a
parameter optimization one with the parameters being the time-scale factor and the
control points of the splines. In Chapter 5, we add another parameter that represents
the payload into the optimization in order to maximize it. Because the PUMA 762
robot has bounded joint velocities, these semi-innite constraints are included in our
formulation. Other researchers have included velocity constraints as penalty functions
[15, 39, 44], or have discretized them into a large number of inequality constraints
throughout the time interval [16], or have used a variable mesh approach [1] to handle
them. As mentioned above, our approach uses the convex hull property of B-splines
in order to transform these semi-innite constraints into a set of linear inequalities
in the parameter space. This provides superior numerical stability since the semi-
innite constraints on joint velocity are satised exactly with a small number linear
constraints on the parameters.
1.2.5 Robotic Body Weight Supported (BWS) Training
In the U.S. alone, over 700,000 people experience a stroke each year, and over
10,000 people experience a traumatic spinal cord injury. Impairment in walking ability
after such neurologic injuries is common. Recently, a new approach to locomotion
rehabilitation called body weight supported (BWS) training has shown promise in
6
Figure 1.1: Body weight supported (BWS) step training following spinal cord injury.
Two therapists assist in leg movement, while a third assists in torso movement. From
Behrman and Harkema (2000).
improving locomotion after stroke and spinal cord injury [62]. The technique involves
suspending the patient in a harness above a treadmill in order to partially relieve the
weight of the body, and manually assisting the legs and pelvis in moving in a walking
pattern (Figure 1.1). Patients who receive this therapy can signicantly increase their
independent walking ability and overground walking speed [5]. It is hypothesized
that the technique works in part by stimulating remaining force, position, and touch
sensors in the legs during stepping in a repetitive manner, and that residual circuits in
the nervous system learn from this sensor input to generate motor output appropriate
for stepping.
Several research groups are pursuing robotic implementations of BWS train-
ing in an attempt to make the training less labor intensive, more consistent, and
more widely accessible [8, 27, 50, 17]. Implementing BWS training with robotics is
7
also attractive because it could improve experimental control over the training, thus
providing a means to better understand and optimize its eects.
A diculty in automating BWS training is that the required patterns of forces
at the pelvis and legs are unknown. For example, the relative importance of assisting
at the pelvis and legs is unclear. One approach toward determining the required
forces is to instrument the therapists hands with force and position transducers [8].
However, therapists are relatively limited in the forces that they can apply compared
to robots, and there is no guarantee that any given therapist has selected an optimal
solution. Current robotic devices for BWS have taken the simplied approach of
replicating leg kinematics, and moving the torso in a gait, like up and down pattern
[17, 26].
In Chapter 6 we explore an alternate approach toward generating strategies
for assisting in gait training: dynamic motion planning. Dynamic motion planning
provides a formalized method for determining motions for underconstrained tasks,
and may reveal novel strategies for achieving the tasks. It has been used with success
to simulate human control over such activities as diving, jumping, and walking [2,
28, 40, 4]. We use dynamic motion planning to determine to what extent repetitive
stepping by a paralyzed person can be generated by assisting in pelvis motion alone.
1.3 Contributions
The following list summarizes the research contributions described in this dis-
sertation. Specically, we:
8
Derived Lie group formulations of hybrid dynamics for both open-chained and
closed-chained robot manipulators.
Developed a system identication method, based on the Lie group formulation of
inverse dynamics, for estimating the dynamic properties of a robot manipulator.
Exploited the convex hull property of B-splines to transform the semi-innite
constraints into a set of linear inequalities.
Solved the free-nal time optimal problems by using a time factor with B-splines.
Derived a weightlifting motion planner for a PUMA 762 robot to extend its
payload capability beyond the limits established by the manufacturer.
Developed a motion planner for the design of a robot used in gait rehabilitation.
This motion planner can also generate leg swing motions similar to the human
gait during the swing phase.
1.4 Outline
This dissertation is organized as follows: in Chapter 2 and Chapter 3, we in-
troduce the essential concepts of kinematics, dierential kinematics and dynamics,
applied particularly to robot manipulators. These concepts, incorporated with Lie
groups and Lie algebras, provide powerful tools for us to generate ecient mathemat-
ical model for robot dynamics.
A function approximation technique is addressed in Chapter 4. The technique
constructs curves using piecewise B-Spline basis functions multiplied by the corre-
sponding control points. Three important properties, 1) local control of curve shape,
9
2) invariance under ane transformation, and 3) convex hull, make the B-Spline curve
a great candidate to parameterize the joint trajectory in motion planning.
Two dynamic motion planning problems are explored in this dissertation: 1)
weightlifting motion planning for a PUMA 762 robot, and 2) motion planning in the
design of robotic gait rehabilitation. In Chapter 5, we develop a dynamic motion
planner for open-chained robots. An example of weightlifting motion planning for a
PUMA 762 robot is discussed. The robot is fully actuated and only inverse dynamics
is needed for computation of the joint torques. A system identication method is
explored in order to realize the dynamics information of a real robot.
The motion planner is used in Chapter 6 to assist in the design of a rehabilitation
robot that can generate gait motions for paralyzed persons. The system model is
underactuated with passive and active joints. The hybrid dynamics consisting inverse
dynamics and forward dynamics is required for the computation of the equations of
motion.
Conclusions and future works are described in Chapter 7.
10
Chapter 2
Kinematics of Robot Manipulators
Kinematics studies the motion without taking into account the forces which
cause it. In this chapter we will address the essential concepts of kinematics and
focus mainly on forward kinematics which computes the position and orientation of
the end-eector based on the conguration of the robot manipulator. Inverse kine-
matics, which nds the conguration to meet the given position and orientation of
the end-eector, will not be addressed. The product-of-exponential (POE) equations
will be introduced to connect the theory of Lie groups and robot forward kinemat-
ics. Dierential kinematics will be discussed last which describes the relationship
between the velocity of the end-eector and the joint rates, and is represented as the
manipulator Jacobian.
2.1 Kinematics
A rigid body is a set of points with the property that the distance between
any two of them never varies. The robot manipulators studied in this dissertation
are dened as collections of rigid bodies, called rigid links, connected by joints. The
position of one link relative to another in a kinematic chain is dened mathemati-
cally by a coordinate transformation between reference frames attached to each body.
11
The Cartesian coordinates are used as natural coordinates for representing point lo-
cations in space, p = (x, y, z). This leads to the following formula for coordinate
transformation:
p
= Rp +d (2.1)
where p
is the position of a rigid point represented in one coordinate frame and

p is the position of the same point represented in another coordinate frame. The
transformation consists a rotation R and a translation d. Because (2.1) does not
satisfy f(p
1
+ p
2
) = f(p
1
) + f(p
2
), it is not a linear transformation. (It is an ane
transformation.)
A modication has been made to obtain a linear transformation. The modi-
ed transformation uses homogeneous coordinates, (p, 1) = (x, y, z, 1). Using these
coordinates yields a linear transformation (homogeneous transformation) on '
4
_
_
p
1
_
_
=
_
_
R d
0 1
_
_
_
_
p
1
_
_
= T
_
_
p
1
_
_
. (2.2)
The matrix T is called homogeneous transformation matrix.
2.1.1 Lie Groups and Lie Algebras
A Lie Group is a dierentiable manifold G such that the following two properties
are satised for A, B G:
Property 1: For the mapping f(A, B) = AB we require f(A, B) G and f
must be continuously dierentiable.
Property 2: The inverse mapping A A
1
must exist and be continuously
dierentiable.
12
Rotation transformations in Euclidean space are accomplished by use of the
orthogonal 33 rotation matrix R. This matrix is an element of the special orthogonal
group on '
3
, SO(3), which is an example of Lie group and is dened as
SO(3) =
_
R[RR
T
= I, det(R) = +1
_
. (2.3)
The special Euclidean group, or SE(3), is a Lie group corresponding to the homoge-
neous transformation on '
3
:
SE(3) =
_
_
_
_
R d
0 1
_
_
[R SO(3), d '
3
_
_
. (2.4)
Hence, in equation (2.2), T SE(3).
An important concept associated with each Lie group is the notion of a Lie
algebra. The tangent space at the identity element of G is the Lie algebra of G and
is denoted by a lowercase g. Associated with the Lie group SO(3) is the Lie algebra
so(3), which can be written in the 3 3 skew-symmetric matrix form
= ()
=
_
_
0
z

y
z
0
x
y

x
0
_
_
(2.5)
or in the 3 1 vector form
= ( )
=
_
z
_
_
(2.6)
where the (wedge) operator and the (vee) operator are dened in Murray [38].
The Lie algebra associated with SE(3) is denoted by se(3), where g se(3) has the
4 4 matrix form
g = (g)
=
_
_

g
v
g
0 0
_
_
(2.7)
13
or the 6 1 vector form
g = ( g)
=
_
g
v
g
_
_
. (2.8)
The principal relationship between the Lie group SE(3) and its corresponding
Lie algebra se(3) is the exponential mapping, which maps se(3) onto SE(3), i.e. for
every G SE(3) there exists a g se(3) such that e
g
= G. The following explicit
formulas for the exponential mappings on so(3) and se(3) are derived in Park [43]
and Murray [38].
Lemma 2.1 Let so(3). Then
exp( ) = I + +
1
2!

2
+
1
3!

3
+
= I +
sin
+
1 cos
2

2
is an element of SO(3), where = [[[[.
Lemma 2.2 Let so(3) and v '
3
. Then
exp(
_
_
v
0 0
_
_
) =
_
_
exp( ) Av
0 1
_
_
is an element of SE(3), where
A = I +
1 cos
2
+
sin
3

2
.
The matrix logarithm takes elements of the group and returns the associated element
of the algebra. It also has a closed-form expression:
Lemma 2.3 Let R SO(3) and d '
3
. Then
log(
_
_
R d
0 1
_
_
) =
_
_
A
1
d
0 0
_
_
14
is an element of se(3), where
=

2 sin
(R R
T
)
A
1
= I
1
2
+
2 sin (1 + cos )
2
2
sin

2
and satises 1 2 cos = Tr(), [[ < . (Also,
2
= [[[[
2
.)
A spatial velocity V is an element of se(3), which can be written either in a
vector from or in a matrix form, such as
V =
_
v
_
_
or

V =
_
_
v
0 0
_
_
(2.9)
where is the angular velocity and v is the linear velocity. While spatial velocities are
regarded as elements of se(3), spatial forces are regarded as inhabiting the associated
dual space se(3)
and can be written as

F =
_
_
m
f
_
_
or

F =
_
f m
0 0
_
_
(2.10)
where m is the moment and f is the force. The dual space se(3)
is the space of
linear functionals on se(3), i.e. if V = (, v) se(3) and F = (m, f) se(3)
, then
F : se(3) ' is given by F(V ) = F
T
V = m
T
+f
T
v.
2.1.2 Adjoint Operators
An element of a Lie group can be used as a linear mapping between its Lie
algebras via the adjoint representation. Given G = (R, d) SE(3) its adjoint map
Ad
G
: se(3) se(3) acting on V = (, v) se(3) is given by
Ad
G
V =
_
_
R 0
dR R
_
_
_
v
_
_
15
= (G
V G
1
)
=
_
_
_
_
_
_
R d
0 1
_
_
_
_
v
0 0
_
_
_
_
R
T
R
T
d
0 1
_
_
_
_
_
_
. (2.11)
Physically, it describes how the spatial velocities transform under a change of reference
frame given by G. Similarly, an element of a Lie algebra can be applied as a linear
mapping between its Lie algebras via its adjoint representation, the Lie bracket. If
V
1
= (
1
, v
1
) se(3), then its adjoint map ad
V
1
: se(3) se(3) on V
2
= (
2
, v
2
)
se(3) is dened as
ad
V
1
V
2
=
_
_

1
0
v
1

1
_
_
_
2
v
2
_
_
= [V
1
, V
2
]
= (
V
1
V
2

V
2
V
1
)
(2.12)
which, physically, generalizes the standard cross product operation in se(3).
The dual operator Ad
G
: se(3)
se(3)
is dened as follows:
Ad
G
F =
_
_
R
T
R
T

d
T
0 R
T
_
_
_
_
m
f
_
_
= (G
1

FG)
=
_
_
_
_
_
_
R
T
R
T
d
0 1
_
_
_
f m
0 0
_
_
_
_
R d
0 1
_
_
_
_
_
_
. (2.13)
Its matrix representation is given by the transpose of Ad
G
. Physically, it describes
how the spatial forces transform under a change of reference frame given by G. The
dual operator ad
V
: se(3)
se(3)
is dened as:
ad
V
F =
_
_

T
v
T
0
T
_
_
_
_
m
f
_
_
= (

F

V

V

F)
. (2.14)
Similar to Ad
G
, its matrix representation is given by the transpose of ad
V
. Physically,
it generalizes the standard cross product operation between se(3) and se(3)
to se(3)
.
16
2.1.3 Forward Kinematics
If a reference frame is xed at the tip of each link in an open kinematic chain,
then the element of SE(3) describing the position and orientation of frame i relative
to that of frame i 1 is T
i1,i
= M
i
e
S
i
q
i
, where M
i
SE(3), S
i
se(3), and q
i
'
is the joint variable for link i. The frame attached at the end-eector is then related
to that of the base by the product
T
0,n
(q
1
, q
2
, , q
n
) = T
0,1
(q
1
)T
1,2
(q
2
) T
n1,n
(q
n
)
= M
1
e
S
r1
q
1
M
2
e
S
r2
q
2
M
n
e
S
rn
q
n
(2.15)
By a repeated application of the matrix identity Me
S
M
1
= e
M

SM
1
, one can obtain
T
0,n
(q
1
, q
2
, , q
n
) = e
A
1
q
1
e
A
2
q
2
e
A
n
q
n
M (2.16)
where A
1
= M
1

S
1
M
1
1
, A
2
= (M
1
M
2
)
S
2
(M
1
M
2
)
1
, etc., and M = M
1
M
2
M
n
.
The conguration with q
i
= 0, i = 1 n, is called the home position. In the home
position, each joint screw A
i
is written in the base frame. The transformation M
relates the end-eector frame to the base frame as well. Alternatively, T
0,n
can also
be expressed as
T
0,n
(q
1
, q
2
, , q
n
) = Me
B
1
q
1
e
B
2
q
2
e
B
n
q
n
(2.17)
where B
i
= M
1
A
i
M, i = 1 n, are the joint screws related to the end-eector
frame in the home position. Equations (2.16) and (2.17) are called the product-of-
exponentials (POE) kinematic equations in space and body coordinates, respectively.
The procedure for computing the forward kinematics of a robot manipulator in
space coordinates is described as follows:
Algorithm 2.1 Forward kinematics via POE in space coordinates:
17
1. Set q
1
= q
2
= = q
n
= 0 so that T
0,n
= M which is the home position.
2. In the home position identify the screw axis direction k
i
and a point on the axis
p
i
.
3. A
i
=
_
k
i
v
i
0 0
_
_
where v
i
= p
i
k
i
+h
i
k
i
; h is the pitch of the screw A
i
.
4. T
0,n
= e
A
1
q
1
e
A
2
q
2
e
A
n
q
n
M.
In the next section, the derivatives of the adjoint mapping will be introduced.
It will be used for computing the derivatives of dynamics and, later, the analytical
gradient in optimization problems.
2.1.4 Derivatives of the Adjoint Mapping
The derivatives of the adjoint operators of Lie groups and Lie algebras are
described as follows:
Lemma 2.4 If the joint transformations are represented in local coordinates, T
i1,i
=
M
i
e
S
i
q
i
, where S
i
is a screw in the coordinates of the local moving frame for link i,
for i
1
i i
2
,
q
i
Ad
T
1
i
1
,i
2
= Ad
T
1
i,i
2
ad
S
i
Ad
T
1
i
1
,i
. (2.18)
First, we see that
q
i
T
i
1
,i
2
= T
i
1
,i1
M
i
e
S
i
q
i
S
i
T
i,i
2
= T
i
1
,i

S
i
T
i,i
2
. (2.19)
Assume V se(3) and take derivative of the following equation with respect to q
i
:
Ad
T
1
i
1
,i
2
V =
_
T
1
i
1
,i
2
V T
i
1
,i
2
_
. (2.20)
18
One can obtain
q
i
Ad
T
1
i
1
,i
2
V =
_
T
1
i,i
2
S
i
T
1
i
1
,i
V T
i
1
,i
2
+T
1
i
1
,i
2
V T
i
1
,i

S
i
T
i,i
2
_
=
_
T
1
i,i
2
(
S
i
T
1
i
1
,i
V T
i
1
,i
T
1
i
1
,i
V T
i
1
,i

S
i
)T
i,i
2
_
= Ad
T
1
i,i
2
ad
S
i
Ad
T
1
i
1
,i
V . (2.21)
Then, writing the above equation in the 6 6 matrix form and removing V , equation
(2.18) is obtained. Similar calculation can be performed to get the following lemma:
i1,i
=
M
i
e
S
i
q
i
, where S
i
for i
1
i i
2
,
q
i
Ad
T
1
i
1
,i
2
= Ad
T
1
i
1
,i
ad
S
i
Ad
T
1
i,i
2
. (2.22)
The derivatives for the ad and ad
operators can be easily obtained using the chain

rule and is therefore omitted.
2.2 Dierential Kinematics
The mapping between the joint velocities and the end-eector velocities is de-
ned by the dierential kinematics equation
V
e
=
_
e
v
e
_
_
= J
e
(q) q (2.23)
where V
e
is the spatial velocity of the end-eector;
e
and v
e
the angular and linear
velocities of the end-eector, respectively; q the generalized velocity of the robot
manipulator; and J
e
the Jacobian matrix. The matrix J
e
is called the geometric
19
Jacobian [19] of the manipulator since its computation follows a geometric procedure
that is based on computing the contribution of each joint velocity to the spatial
velocity of the end-eector.
Consider the forward kinematics map, T
0,n
: Q SE(3), in (2.15) for a robot
manipulator. A point p xed in the end-eector can be represented in the base frame
as
_
_
p
1
_
_
= T
0,n
_
_
p
1
_
_
=
_
_
R
e
d
e
0 1
_
_
_
_
p
1
_
_
. (2.24)
The spatial velocity V
g
e
of the end-eector written in the base frame can be mapped
from the joint velocities q by the geometric Jacobian J
g
e
(q):
V
g
e
=
_
g
e
v
g
e
_
_
=
_
_
(R
T
e
R
e
)
d
e
_
_
= J
g
e
(q) q . (2.25)
Two variations of J
g
e
can also be expressed in similar forms, depending on whether
the velocity of the point is measured in the base frame ( p
) or in the end-eector
frame ( p).
2.2.1 The Manipulator Jacobian
In this section we introduce two Jacobian matrices, the spatial Jacobian and
the body Jacobian. They are similar to J
g
e
, but are represented in dierent coordinate
frames. The spatial Jacobian is represented in the base frame while the body Jacobian
is represented in the end-eector frame.
20
The velocity of point p xed in the end-eector, is obtained as follows by taking
the derivative of (2.24):
_
_
p
0
_
_
=

T
0,n
_
_
p
1
_
_
=

T
0,n
T
1
0,n
_
_
p
1
_
_
=
_
R
e

d
e
0 0
_
_
_
_
R
T
e
R
T
e
d
e
0 1
_
_
_
_
p
1
_
_
=
_
R
e
R
T
e
d
e

R
e
R
T
e
d
e
0 0
_
_
_
_
p
1
_
_
=
_
_

g
e
d
e
g
e
d
e
0 0
_
_
_
_
p
1
_
_
(2.26)
where

T
0,n
T
1
0,n
is the map between the position and velocity of the end-eector in the
base frame. If the joints move along a path q(t) Q '
n
, then the end-eector
traverses a path T
0,n
SE(3). The instantaneous spatial velocity of the end-eector
is given by the twist
V
s
e
= (

T
0,n
T
1
0,n
)
i
(
T
0,n
q
i
T
1
0,n
)
q
i
= J
s
e
(q) q (2.27)
where J
s
e
'
6n
is called the spatial Jacobian. The formulas for the derivative and
inverse of T
0,n
are:
T
0,n
q
i
= M
1
e
S
1
q
1
M
i
e
S
i
q
i
S
i
M
i+1
e
S
i+1
q
i+1
M
n
e
S
n
q
n
(2.28)
T
1
0,n
= e
S
n
q
n
M
1
n
e
S
n1
q
n1
M
1
n1
e
S
1
q
1
M
1
1
(2.29)
which produce the product
T
0,n
q
i
T
1
0,n
= M
1
e
S
1
q
1
M
i
e
S
i
q
i
S
i
e
S
i
q
i
M
1
i
e
S
1
q
1
M
1
1
= T
0,i

S
i
T
1
0,i
=
_
Ad
T
0,i
S
i
_
(2.30)
21
and the Jacobian can be expressed entirely in terms of the joint screws mapped into
the base frame
J
s
e
(q) = [Ad
T
0,1
S
1
Ad
T
0,2
S
2
Ad
T
0,n
S
n
] . (2.31)
Each column of J
s
e
(q) depends only on q
1
, q
2
, , q
i1
. In other words, the contribu-
tion of the i
th
joint velocity to the end-eector velocity is independent of the later
conguration in the manipulator.
i1,i
=
M
i
e
S
i
q
i
, where S
i
then the spatial Jacobian is J
s
e
= [Ad
T
0,1
S
1
Ad
T
0,2
S
2
Ad
T
0,n
S
n
] .
Rewrite (2.26) and represent the position and velocity in the end-eector frame.
One obtains the following equation:
T
1
0,n
_
_
p
0
_
_
= T
1
0,n
T
0,n
_
_
p
1
_
_
=
_
_
R
T
e
R
e
R
T
e
d
e
0 0
_
_
_
_
p
1
_
_
. (2.32)
The body Jacobian J
b
e
is dened by the relationship
V
b
e
= (T
1
0,n
T
0,n
)
i
(T
1
0,n
T
0,n
q
i
)
q
i
= J
b
e
(q) q . (2.33)
A calculation similar to that performed previously yields
J
b
e
(q) =
_
Ad
T
1
1,n
S
1
Ad
T
1
2,n
S
2
Ad
T
1
n1,n
S
n1
S
n
_
. (2.34)
The body Jacobian can be expressed entirely in terms of the joint screws mapped
into the end-eector frame. It maps the joint velocities into the corresponding spatial
velocity of the end-eector in the end-eector frame.
22
i1,i
=
M
i
e
S
i
q
i
, where S
i
then the body Jacobian is J
b
e
=
_
Ad
T
1
1,n
S
1
Ad
T
1
2,n
S
2
Ad
T
1
n1,n
S
n1
S
n
_
.
The spatial and body Jacobians are related by the adjoint transformation:
J
s
e
(q) = Ad
T
0,n
J
b
e
(q) . (2.35)
Both the spatial and body Jacobians can be related to the geometric Jacobian
in (2.25) by a coordinate transformation. The 4 4 matrix form of V
g
e
in (2.25) is
_
_

g
e
d
e
0 0
_
_
=
_
_
I d
e
0 1
_
_
_
_

g
e
d
e
g
e
d
e
0 0
_
_
_
_
I d
e
0 1
_
_
= G
1
s
T
0,n
T
1
0,n
G
s
=
_
Ad
G
s
1J
s
e
q
_
. (2.36)
Therefore, the relation between J
g
e
and J
s
e
can be expressed as
J
g
e
= Ad
G
s
1J
s
e
. (2.37)
Similar procedure can be performed to obtain the the relation between J
g
e
and J
b
e
:
J
g
e
= Ad
G
b
J
b
e
(2.38)
where
G
b
=
_
_
R
e
0
0 1
_
_
.
2.3 Summary
The concepts of both kinematics and dierential kinematics are studied in this
chapter. Kinematics can be represented as joint motions in the special Euclidean
23
group, SE(3), which is a Lie group corresponding to homogeneous transformations,
while dierential kinematics can be represented as se(3) which is the Lie algebra
associated with SE(3). The Lie algebra is the tangent space at the identity element
of the Lie group. Mapping with the adjoint operators, the Lie group and the Lie
algebra can be used to express not only the propagation of the spatial velocities and
accelerations but also the propagation of the spatial forces of a robot manipulator.
The propagation of the spatial velocities, accelerations and forces is referred to as the
recursive formulation of dynamics which will be addressed in the next chapter.
24
Chapter 3
Dynamics of Robot Manipulators
Dynamics of mechanisms is a eld in which much research eort has been made.
In this chapter, the dynamics of robot manipulators will be studied, such as inverse
dynamics, forward dynamics and hybrid dynamics including inverse and forward dy-
namics. Formulations for both open-chained and closed-chained systems will be intro-
duced. These formulations, incorporated with Lie groups and Lie algebras, provide
powerful tools for us to generate ecient mathematical models of robot dynamics.
3.1 Open-Chained Systems
The dynamic equations of open-chained robot manipulators can be expressed
in the general form
H(q) q +h(q, q) = (3.1)
which relates the applied joint forces to the joint positions q and their time deriva-
tives q and q. H(q) is the mass or inertia matrix and h(q, q) contains the centrifugal,
Coriolis, gravitational and frictional forces.
The dynamics problem for robot manipulators is divided into two sub-problems,
inverse dynamics and forward dynamics. The inverse dynamics problem aims at
25
determining the motor or driving forces that produce a specic motion, as well as
the reactions that appear at each joint of the robot manipulator. It is necessary to
know the velocities and accelerations to be able to estimate the inertia forces which,
together with the weight and all the other known external forces, will provide the
basis to calculate the required actuating forces.
The forward dynamics problem determines the acceleration, given the applied
forces. These are used to nd the motion of a robot manipulator over a given time
interval, given some initial conditions. The importance of the forward dynamic prob-
lem lies in the fact that it allows one to simulate and predict the systems actual
behavior; the motion is always the result of the forces that produce it.
3.1.1 Inverse Dynamics
The solution to the inverse dynamics has dierent applications. In the rst place,
it determines the forces to which the system is subjected. Extremely important is the
fact that the inverse dynamics yields the driving forces necessary to control a system
so that it follows a desired trajectory.
An ecient Lie group formulation given below supplies the solution to the in-
verse dynamics problem [42, 45]. It is of order O(n) where n is the number of the
joints.
Algorithm 3.1 The Newton-Euler recursive formulation of the inverse dynamics is:
26
Initialization
V
0
,

V
0
, F
n+1
Forward recursion: i = 1 to n
V
i
= Ad
T
1
i1,i
V
i1
+S
i
q
i
V
i
= Ad
T
1
i1,i
V
i1
+S
i
q
i
+ad
V
i
S
i
q
i
Backward recursion: i = n to 1
F
i
= Ad
T
1
i,i+1
F
i+1
+J
i

V
i
ad
V
i
J
i
V
i
i
= S
T
i
F
i
+f
ci
sgn( q
i
) +f
vi
q
i
In the algorithm, the index i represents the i
th
link frame counted from the base
frame (i = 0). The joint screw, spatial velocity, spatial acceleration and spatial force
are written as S, V ,

V and F se(3), respectively. Particularly, V
0
and

V
0
represent
the spatial velocity and acceleration of the base, respectively, while F
n+1
represents
the external spatial force on the last link or end-eector. T
i1,i
SE(3) denotes the
transformation from the i 1
th
link frame to the i
th
link frame. The joint velocity,
acceleration, force and the Coulomb and viscous frictions are written as q, q, , f
c
and f
v
', respectively. And J is the spatial inertia matrix
J
i
=
_
_
I
i
m
i
r
2
i
m
i
r
i
m
i
r
i
m
i
1
_
_
(3.2)
where m
i
and I
i
are the mass and inertia of the i
th
link, respectively; r
i
is a vector
from the origin of the i
th
link frame to the center of mass of i
th
link; r
i
is the skew-
symmetric matrix formed by r
i
using the notation from the last chapter; and 1 is an
27
identity matrix. The spatial velocity and force are
V
i
=
_
i
v
i
_
_
and F
i
=
_
_
m
ti
f
ti
_
_
(3.3)
where , v, m
t
andf
t
are the angular velocity, linear velocity, moment and force,
respectively. This recursive formulation shows how the spatial velocity and accelera-
tion propagate forwards from the base to the end-eector and how the spatial force
propagates backwards from the end-eector to the base.
3.1.2 Forward Dynamics
The forward dynamics denes a system of nonlinear ordinary dierential equa-
tions (initial value problem). The solution to these dierential equations can be found
by numerically integrating them starting from the initial conditions. An important
characteristic of this mathematical problem is that it is computationally intensive.
Because of this, it is very important to choose the most ecient method for dealing
with and solving this problem. Featherstones [23] articulated-body method supplies
an ecient solution to the forward dynamics problem.
Featherstone has shown that the equations of motion of each link can be ex-
pressed as follows:
F
i
=

J
i

V
i
+

F
i
(3.4)
where

J
i
is the articulated-body inertia of link i, and

F
i
is the bias force associated
with link i. To this end, consider the articulated body consisting the last two links of
an n link chain. We assume that a spatial force F
n1
is applied to the handle of the
articulated body. From Algorithm 3.1, it follows the equations of motion of each link
F
n
= J
n

V
n
ad
V
n
J
n
V
n
(3.5)
28
F
n1
= Ad
T
1
n1,n
F
n
+J
n1

V
n1
ad
V
n1
J
n1
V
n1
(3.6)
Note that the equations of motion for link n are already in the form of (3.4) with
J
n
= J
n
and

F
n
= ad
V
n
J
n
V
n
. Referring to Algorithm 3.1, the spatial acceleration
of the last link is given by
V
n
= Ad
T
1
n1,n
V
n1
+S
n
q
n
+ad
V
n
S
n
q
n
(3.7)
Substituting it into (3.5) yields
F
n
= J
n
Ad
T
1
n1,n
V
n1
+J
n
S
n
q
n
+J
n
ad
V
n
S
n
q
n
ad
V
n
J
n
V
n
(3.8)
Recalling that
n
= S
T
n
F
n
it is straightforward to show that
q
n
=
n
S
T
n
(J
n
Ad
T
1
n1,n
V
n1
+J
n
ad
V
n
S
n
q
n
ad
V
n
J
n
V
n
)
S
T
n
J
n
S
n
(3.9)
Substituting (3.9) back to (3.8), F
n
can be written as a new equation
F
n
= J
n
Ad
T
1
n1,n
V
n1
+J
n
ad
V
n
S
n
q
n
ad
V
n
J
n
V
n
+
J
n
S
n
n
S
T
n
(J
n
Ad
T
1
n1,n
V
n1
+J
n
ad
V
n
S
n
q
n
ad
V
n
J
n
V
n
)
S
T
n
J
n
S
n
(3.10)
in which only

V
n1
of the spatial accelerations shows rather than

V
n
. Substituting
the new equation of F
n
to (3.6) and rearranging it yields
F
n1
=

J
n1

V
n1
+

F
n1
. (3.11)
Sequential computation from n to 1 conrms (3.4).
In general, the articulated-body method executes the forward computation of
the spatial velocities, the backward computation of the articulated-body inertias and
the bias forces, and then the forward computation of the joint accelerations and the
spatial accelerations. A general algorithm for the system including both inverse and
forward dynamics will be introduced in the next section.
29
3.1.3 Hybrid Dynamics
In this dissertation we dene the hybrid dynamics problem as a system of equa-
tions for articulated bodies in which either the applied force or the joint acceleration
for each joint is known. For such a system, the computation of either the inverse
dynamics or the forward dynamics is needed for each joint depending on which one
of
i
and q
i
is specied for the joint.
The articulated-body method can be applied to solve not only the forward dy-
namics but also the inverse dynamics. A similar procedure to derive the formulation
for forward dynamics is used for the inverse dynamics. Consider the articulated body
in the previous section. Instead of the applied forces, the joint accelerations are spec-
ied. In this case, F
n
can be calculated directly from (3.8) without any knowledge
of

V
n
. Substituting (3.8) into (3.6) and rearranging it conrms (3.11). Note that the
equations for computing

J
n1
and

F
n1
are dierent from those in the forward dy-
namics. Sequential computation from n to 1 conrms the articulated equation (3.4).
Compared to the Newton-Euler recursive formulation with two iterations, there are
three iterations needed for the articulated-body formulation. Although it is less e-
cient than the Newton-Euler recursive formulation for the inverse dynamics case, the
articulated-body method supplies a powerful tool to deal with the complex dynamics
problem involving both inverse dynamics and forward dynamics.
Algorithm 3.2 The articulated-body recursive formulation of the hybrid dynamics
is:
Initialization
V
0
,

V
0
, F
n+1
, F
bn+1
= F
n+1
,

J
n+1
= 0, J
bn+1
= 0, b
n+1
= 0
30
V
i
= Ad
T
1
i1,i
V
i1
+S
i
q
i
J
i
= J
i
+Ad
T
1
i,i+1
J
bi+1
Ad
T
1
i,i+1
F
i
= ad
V
i
J
i
V
i
+Ad
T
1
i,i+1
(F
bi+1
+

J
i+1
S
i+1
b
i+1
)
F
bi
=

F
i
+

J
i
ad
V
i
S
i
q
i
J
bi
=
_
J
i
i I
a
J
i

J
i
S
i
S
T
i
J
i
/(S
T
i
J
i
S
i
) i I
p
b
i
=
_
_
q
i
i I
a
(
i
S
T
i
F
bi
)/(S
T
i
J
i
S
i
) i I
p
_
i
= S
T
i
(F
bi
+

J
i
Ad
T
1
i1,i
V
i1
+

J
i
S
i
b
i
) i I
a
q
i
= b
i
S
T
i
J
i
Ad
T
1
i1,i
V
i1
/(S
T
i
J
i
S
i
) i I
p
V
i
= Ad
T
1
i1,i
V
i1
+S
i
q
i
+ad
V
i
S
i
q
i
The variables used here are the same as in Algorithm 3.1. The set of active joints
in which the forward dynamics is applied is represented as I
a
; and the set of passive
joints in which the inverse dynamics is applied is represented as I
p
. Furthermore, for
a branched-chained system, (3.4) can be extended to be
F
i
=
(

J
i

V
i
+

F
i
) . (3.12)
The formulation is very similar to Algorithm 3.2 and will not be addressed.
31
3.2 Closed-Chained Systems
So far the systems we have considered are open-chained robot manipulators. An
example of such a system is a robot arm moves around without any contact with the
environment. If there is any contact with the environment, it forms an closed-chained
system which can also be viewed as an open-chained system with the contact forces.
Lets consider the closed-chained system with the following holonomic constraint
equations, : '
n
'
m
:
(q(t)) = 0 (3.13)
during some time interval. The time derivatives of it can be simply derived as
=
q
q = 0 (3.14)
=
q
q +

q
q = 0 (3.15)
where
q
can be computed in a similar way to the geometric Jacobian described in
Section 2.2. The torque,
c
, reecting on each joint due to the contact between the
robot and the environment, can be obtained by
c
=
T
q
F
c
(3.16)
in which F
c
is the contact force (or the Lagrange multiplier). Incorporating it with
the open-chained system (3.1) forms the following closed-chained system:
H(q) q +h(q, q) = +
T
q
F
c
. (3.17)
Together with (3.15) it can be written as
_
_
H
T
q
q
0
_
_
_
_
q
F
c
_
_
=
_
_
h
q
q
_
_
(3.18)
32
which is a system of (n + m) equations with (n + m) unknowns, whose matrix is
symmetrical and, in general, non-positive denite, and also very sparse in many
practical cases.
Equations (3.18) can be used for the simultaneous solution of the accelerations
and the contact forces. Alternatively, (3.17) can be solved rst to obtain an expression
for the accelerations:
q = H
1
( h +
T
q
F
c
) (3.19)
which can only be used if the mass matrix is non-singular, as it will be in most cases.
By substituting (3.19) into (3.15) one obtains
F
c
= (
q
H
1
T
q
)
1
(
q
H
1
(h )

q
q) . (3.20)
In order to calculate the accelerations, it suces to substitute F
c
in (3.19).
Equation (3.18) uses the kinematic acceleration equations (3.15) obtained by
dierentiating the constraint equations (3.13) twice with respect to time. This means
that when integrating the system dynamics, the following dierential equation must
also be integrated with respect to time:
(q(t)) =
q
q +

q
q = 0 . (3.21)
This system of dierential equations has the following solution:
(t) = a
1
t +a
2
(3.22)
where a
1
and a
2
are constant vectors that depend on the initial conditions. If the
initial position and velocity satisfy the constraint equations, both vectors a
1
and a
2
are
null. Theoretically, (3.22) guarantees that the constraint equations will be satised
at any time. In practice, this is not true.
33
Equation (3.21) is unstable since for any vector a
1
dierent from zero the general
solution given by equation (3.22) is not bounded and tends to increase indenitely
with time. Even though the initial conditions guarantee that a
1
= 0, during the course
of the numerical integration, the round-o errors that appear during the integration
do not satisfy the constraint equations. The eects of these errors increase with time,
in accordance with expression (3.22).
The instability during the numeric integration of kinematic acceleration equa-
tions ensures that (3.18) may not be used directly to obtain the solution of the dy-
namic simulation problem. The Baumgarte stabilization method [7] is used to replace
the dierential constraint equations by the following system:
+k
1

+k
2
=
q
q +

q
q +k
1
q
q +k
2
= 0 (3.23)
where k
1
and k
2
are appropriately chosen constants. The general solution to this
dierential equation is
= a
1
e
s
1
t
+a
2
e
s
2
t
(3.24)
where a
1
and a
2
are constant vectors that depend on the initial conditions and s
1
and
s
2
are the roots of the characteristic equation, s
2
+k
1
s+k
2
= 0. If k
1
and k
2
are positive
constants, the two roots have a real negative part which guarantees the stability of
the general solution (3.23) in contrast with that in (3.21). The initial condition of the
system should guarantee that the vectors a
1
and a
2
are zero. If numerical round-o
errors alter this condition, the real negative part of the exponential terms damps
out the possible errors occurring during the integration process. From our numerical
experiments, it appears that the behavior of the method does not signicantly depend
on the value of k
1
and k
2
.
34
By using (3.23) instead of (3.21) the closed-chained system (3.18) is transformed
into
_
_
H
T
q
q
0
_
_
_
_
q
F
c
_
_
=
_
_
h
q
q +k
1
q
q +k
2
_
(3.25)
which produces acceptable results. The Baumgarte stabilization approach is general,
simple and numerically ecient.
3.2.1 Lathrops Algorithm
An alternative recursive formulation of the dynamics of constrained robot sys-
tems was derived by Lathrop [31]. Two 6-vectors,
n
and

0
, of unknown endpoint
degrees of freedom are introduced where an overbar indicates quantities related to
base constraints. The endpoint spatial acceleration,

V
0
and

V
n
, and force, F
0
and
F
n+1
, can be written as some known linear combination of

0
and
n
as shown in the
following algorithm where

M
a
0
,

M
f
0
, M
a
n
, M
f
n+1
,
a
0
,
f
0
,
a
n
, and
f
n+1
are known. The
inuence of
n
on the base is computed by a backward recursion which is related to
0
to obtain
0
. Then
i
and q
i
are solved with a forward computation.
Algorithm 3.3 The recursive formulation of the dynamics of the constrained robot
system is:
Initialization
V
0
V
0
=

M
a
0
0
+
a
0
; F
0
=

M
f
0

0
+
f
0
V
n
= M
a
n
n
+
a
n
; F
n+1
= M
f
n+1
n
+
f
n+1
M
f
n
= Ad
T
1
n,n+1
M
f
n+1
+J
n
M
a
n
35
f
n
= Ad
T
1
n,n+1
f
n+1
+J
n
a
n
ad
V
n
J
n
V
n
F
n
= M
f
n
n
+
f
n
V
i
= Ad
T
1
i1,i
V
i1
+S
i
q
i
i I
a
M
a
i1
= Ad
T
i1,i
M
a
i
a
i1
= Ad
T
i1,i
(
a
i
ad
V
i
S
i
q
i
S
i
q
i
)
M
f
i1
= Ad
T
1
i1,i
M
f
i
+J
i1
M
a
i1
f
i1
= Ad
T
1
i1,i
f
i
+J
i1
a
i1
ad
V
i1
J
i1
V
i1
i I
p
M
i
= 1
66
e
j
S
T
i
M
f
i
[S
T
i
M
f
i
]
j
;
i
= e
j
i
S
T
i

f
i
[S
T
i
M
f
i
]
j
M
a
i
= M
a
i
M
i
;
a
i
= M
a
i

i
+
a
i
M
f
i
= M
f
i
M
i
;
f
i
= M
f
i

i
+
f
i
M
a
i1
= Ad
T
i1,i
(M
a
i
S
i
e
T
j
)
a
i1
= Ad
T
i1,i
(
a
i
ad
V
i
S
i
q
i
)
M
f
i1
= Ad
T
1
i1,i
M
f
i
+J
i1
M
a
i1
f
i1
= Ad
T
1
i1,i
f
i
+J
i1
a
i1
ad
V
i1
J
i1
V
i1
Solve
0
for:
V
0
=

M
a
0
0
+
a
0
; F
0
=

M
f
0

0
+
f
0
V
0
= M
a
0
0
+
a
0
; F
0
= M
f
0
0
+
f
0
36
i I
a
i
=
i1
i I
p
q
i
= e
T
j

i1
i
=
i1
e
j
q
i
i
= M
i
+
V
i
= Ad
T
1
i1,i
V
i1
+S
i
q
i
+ad
V
i
S
i
q
i
In the algorithm, [x]
j
denotes the j
th
element of x and e
j
is the j
th
unit vector (all
the elements equal to zero except the j
th
element equal to one).
In the next section, formulations of derivatives of dynamics will be introduced.
The formulations will be applied in the next chapter to compute the analytical gra-
dient for a optimization problem.
3.3 Derivatives of Dynamics
Assume that the joint trajectories are parameterized as q = q(t, p). The follow-
ing formulation can be obtained by taking derivative of the inverse dynamics with
respect to p [35]. (Here we dont take into account friction, since the Coulomb friction
term is not continuously dierentiable.)
37
Algorithm 3.4 The recursive formulation of the derivative of the inverse dynamics
is:
Initialization
V
0
p
,

V
0
p
,
F
n+1
p
V
i
p
= Ad
T
1
i1,i
V
i1
p
+S
i
q
i
p
+ad
V
i
S
i
q
i
p

V
i
p
= ad
Ad
T
1
i1,i
V
i1
S
i
q
i
p
+Ad
T
1
i1,i

V
i1
p
+
S
i
q
i
p
+adV
i
p
S
i
q
i
+ad
V
i
S
i
q
i
p
F
i
p
= Ad
T
1
i,i+1
F
i+1
p
+J
i

V
i
p
ad
V
i
p
J
i
V
i
ad
V
i
J
i
V
i
p
Ad
T
1
i,i+1
ad
S
i+1
q
i+1
p
F
i+1
i
p
= S
T
i
F
i
p
In the example of a PUMA robot described in the next chapter, the last link
holds the payload, which is the parameter p
w
in (6.1). Therefore J
n
= J
n
(p
w
), and
the derivative of the inverse dynamics with respect to p
w
is described below:
Algorithm 3.5 Replace J
i
in Algorithm 3.1 by
J
i
=
_
_
0 if i = 1 n 1
J
n
/p
w
if i = n
38
The derivatives of the joint torques and/or accelerations in the hybrid dynamics
for both open-chained and closed-chained systems are more complicated but can be
obtained by a similar approach to that used for the derivative of the inverse dynamics.
These formulations are not addressed in this dissertation. The formulation for the
hybrid dynamics of the open-chained system can be found in Sohl and Bobrow [53].
These derivatives can be used to compute the analytical gradient of the dynamic
motion planning in the next two chapters.
3.4 Summary
In this chapter, the dynamics of robot manipulators is studied for both open-
chained and closed-chained systems. The dynamics of the open-chained robot can
be computed with an ecient recursive formulation. In this dissertation, inverse
dynamics will be applied to weight-lifting motion planning for a PUMA 762 robot,
while hybrid dynamics consisting of inverse dynamics and forward dynamics will
be applied to the motion planning for a walking rehabilitation robot. These two
applications of motion planning will be addressed in the following chapters.
39
Chapter 4
B-Spline Curves
The B-spline curve is a piecewise polynomial curve dened by a set of control
points which the curve ordinarily does not interpolate. The B-spline basis function,
which is nonzero in one region and zero otherwise, serves to construct the B-spline
curves by summing up the multiplications of each basis and its associated control
point. The degree of its polynomial is dened independently of the number of control
points. Local control of curve shape is possible because changes in control point
location do not propagate shape change globally, and control points inuence only a
few of the nearby curve segments. A valuable property of B-splines is that they are
invariant under ane transformations (translation, rotation, scaling and/or shear).
These properties make the B-spline a great candidate for curve approximation.
4.1 Construction of B-Splines
A curve through time can be approximated by a piecewise set of B-spline poly-
nomials over the knot space of an ordered time sequence. A particular property of
B-spline curves is local controlaltering the position of a single data point causes
only a part of the curve to change. An example of a cubic B-spline curve is shown
in Figure 4.1. And the local control of B-splines is demonstrated in Figure 4.2 where
40
0 1 2 3 4
1.5
1
0.5
0
0.5
1
1.5
t (knot)
q
Figure 4.1: A cubic B-spline curve (solid line) constructed on the basis functions
(dashed lines).
0 2 4 6 8 10
3
2
1
0
1
2
3
4
5
6
t
q
Figure 4.2: Local control of the B-spline curves.
41
a cubic B-spline curve is shown as the solid line with control points (circle marks),
3,1,5,-2,p
= 2,-1,1, over the knot space 0,1,2, ,10. Moving the control point
p
from 2 to 5, the curve becomes the dashed line whose shape is only changed
locally in the region [4, 8].
Letting the knot space be t
0
t
k1
t
k
t
m
t
m+1

t
m+k
, the B-spline curve, q ', is written as
q(t, P) =
m
j=0
p
j
B
j,k
(t) (4.1)
where P = p
0
, , p
m
, with p
j
', are the control points and B
j,k
is the B-spline
basis function. The index k denes the order of the basis polynomial, e.g. k = 4
for a cubic one or k = 6 for a quintic one. (If B
j,k
has no repeated knots, it is C
k2
continuous, i.e. continuous in the k 2
th
order derivative.) Equation (4.1) is a special
case of the Non-Uniform Rational B-Spline (NURBS) curves dened by
q(t, P) =
m
j=0
h
j
p
j
B
j,k
(t)
m
j=0
h
j
B
j,k
(t)
(4.2)
where h
j
s are the weights. If h
j
= 1 for all j, then since

m
j=0
B
j,k
(t) = 1 (the convex
hull property of B-splines), (4.2) reduces to the nonrational B-spline form in (4.1).
4.1.1 B-Spline Basis Functions
A recursive denition of the divided dierence operator is given as follows [6].
Denition 4.1 For any values t
i
t
i+l
the l
th
divided dierence is given by
[t
i
(l) : x]f(x) = f(t
i
) (4.3)
42
for l = 0, and for l 1 by
[t
i
(l) : x]f(x) =
_
_
[t
i+1
(l1):x]f(x)[t
i
(l1):x]f(x)
t
i+l
t
i
if t
i
l
,= t
i
1
l!
d
l
dx
l
f(x) [
x=t
i
if t
i+l
= t
i
.
(4.4)
And we dene a plus operator, ()
+
, such that
()
+
=
_
_
() if () 0
0 if () < 0 .
(4.5)
A B-spline basis function is given by the following denition.
Denition 4.2 Given knots t
0
, , t
m
, , t
m+k
, and assuming that j m, the B-
spline of order k associated with the knots t
j
, , t
j+k
is given by
B
j,k
(t) = (1)
k
(t
j+k
t
j
)[t
j
(k) : x](t x)
k1
+
. (4.6)
It should be observed that B
j,k
(t) is vacuous if t
j
= t
j+k
.
The basis function can be obtained by a recursive formulation.
Algorithm 4.1 The recursion for B-spline is:
B
j,k
(t) =
t t
j
t
j+k1
t
j
B
j,k1
(t) +
t
j+k
t
t
j+k
t
j+1
B
j+1,k1
(t) (4.7)
starting with B
j,1
(t) =
_
_
1 if t
j
t < t
j+1
0 otherwise .
Figure 4.3 gives a functional picture of the recurrence producing a quadratic
B-spline. The hat function B
i,2
(t) results from adding together a linear factor times
the step function B
i,1
(t) and a linear factor times the step function B
i+1,1
(t). The
hat function B
i+1,2
(t) is produced similarly from linear factors and the step func-
tions B
i+1,1
(t) and B
i+2,1
(t). Finally, the quadratic hump B
i,3
(t) results from adding
together a linear factor times B
i,2
(t) and a linear factor times B
i+1,2
(t).
43
B
i,1
(t) B
i+1,1
(t) B
i+2,1
(t)
B
i,2
(t) B
i+1,2
(t)
B
i,3
(t)
Figure 4.3: A quadratic B-spline produced from the recurrence.
44
2 3 4 5 6 7 8 9
2.5
2
1.5
1
0.5
0
0.5
1
1.5
2
2.5
t
q
Figure 4.4: A step-function B-spline curve with the control points 1.5, 1, 2, 1
over the knots 2, 4, 5, 7.5, 9.
4.2 Examples of B-Splines
A simple example of a B-spline is the step-function B-spline with k = 1. Using
(4.6) its basis function can be represented as
B
j,1
(t) = (1)(t
j+1
t
j
)[t
j
(1) : x](t x)
0
+
= (1)(t
j+1
t
j
)
(t t
j+1
)
0
+
(t t
j
)
0
+
t
j+1
t
j
= (t t
j
)
0
+
(t t
j+1
)
0
+
=
_
_
0 for t < t
j
1 for t
j
t < t
j+1
0 for t
j+1
t .
(4.8)
This B-spline basis function serves to construct piecewise, rst-order, C
1
polynomi-
als, i.e. step functions. Figure 4.4 shows a step-function B-spline curve
q(t) =
3
j=0
p
j
B
j,1
(t)
45
with the control points p
0
= 1.5, p
1
= 1, p
2
= 2, p
3
= 1 over the knots t
0
=
2, t
1
= 4, t
2
= 5, t
3
= 7.5, t
4
= 9.
A B-spline basis function with k = 2 is a hat function which can be written as
B
j,2
(t) = (1)
2
(t
j+2
t
j
)[t
j
(2) : x](t x)
1
+
= (t
j+2
t
j
)
[t
j+1
(1) : x](t x)
1
+
[t
j
(1) : x](t x)
1
+
t
j+2
t
j
=
(t t
j+2
)
1
+
(t t
j+1
)
1
+
t
j+2
t
j+1
(t t
j+1
)
1
+
(t t
j
)
1
+
t
j+1
t
j
=
_
_
0 for t < t
j
tt
j
t
j+1
t
j
for t
j
t < t
j+1
t
j+1
t
t
j+2
t
j+1
+ 1 for t
j+1
t < t
j+2
0 for t
j+2
t
(4.9)
for t
j
< t
j+1
< t
j+2
, and for t
j
< t
j+1
= t
j+2
B
j,2
(t) = (1)
2
(t
j+2
t
j
)[t
j
(2) : x](t x)
1
+
=
d
dx
(t x)
1
+
[
x=t
j+1
=t
j+2
[t
j
(1) : x](t x)
1
+
= (t t
j+1
)
0
+
(t t
j+1
)
1
+
(t t
j
)
1
+
t
j+1
t
j
=
_
_
0 for t < t
j
tt
j
t
j+1
t
j
for t
j
t < t
j+1
0 for t
j+1
t .
(4.10)
A piecewise linear B-spline curve
q(t) =
4
j=0
p
j
B
j,2
(t)
is shown in Figure 4.5 with the control points p
0
= 2, p
1
= 1, p
2
= 3, p
3
= 0, p
4
=
1.5 over the knots t
0
= 1, t
1
= 2.5, t
2
= 3.5, t
3
= 6, t
4
= 6, t
5
= 7.5, t
6
= 10. The
discontinuity at t = 6 is due to the two repeated knots, t
3
and t
4
.
46
2 4 6 8 10
3
2
1
0
1
2
3
4
t
q
Figure 4.5: A piecewise linear B-spline curve with the control points 2, 1, 3, 0, 1.5
over the knots 1, 2.5, 3.5, 6, 6, 7.5, 10.
4.3 Derivatives of B-Splines
The basis function can also be viewed as a piecewise function as follows:
B
j,k
(t) =
_
_
g
jl
(t) if t
j+l
t < t
j+l+1
(l = 0 k 1)
0 if t < t
j
or t t
j+k
(4.11)
where g
jl
is a k 1
th
order polynomial given in [6].
The derivative of B
j,k
with respect to time can be obtained from (4.7) as
t
B
j,k
(t) = (k 1)
_
B
j,k1
(t)
t
j+k1
t
j
B
j+1,k1
(t)
t
j+k
t
j+1
_
. (4.12)
Hence, the rst derivative of q with respect to time is
t
q(t) =
m
j=0
p
j
t
B
j,k
(t)
= (k 1)
_
_
_
m
j=0
p
j
B
j,k1
(t)
t
j+k1
t
j
j=0
p
j
B
j+1,k1
(t)
t
j+k
t
j+1
_
_
_
47
= (k 1)
_
_
_
m
j=0
p
j
B
j,k1
(t)
t
j+k1
t
j
m+1
j=1
p
j1
B
j,k1
(t)
t
j+k1
t
j
_
_
_
= (k 1)
m+1
j=0
p
j
p
j1
t
j+k1
t
j
B
j,k1
(t) (4.13)
and the second derivative of q with respect to time is
2
t
2
q(t) = (k 1)(k 2)
m+2
j=0
_
p
j
p
j1
t
j+k1
t
j
p
j1
p
j2
t
j+k2
t
j1
_
B
j,k2
(t)
t
j+k2
t
j
(4.14)
where p
2
, p
1
, p
m+1
and p
m+2
(for j = 0, 1, m + 1, m + 2, respectively) are dened
as zero. Higher order derivatives of q(t) can be computed consequently following a
similar procedure.
Referring to the convex hull property of B-splines
B
j,k
(t) 0 and
m
j=0
B
j,k
(t) = 1 (4.15)
we see that if q q(t) q,
m
j=0
p
j
B
j,k
(t) = q(t) q = q
m
j=0
B
j,k
(t) =
m
j=0
qB
j,k
(t) . (4.16)
Because B
j,k
is always positive by denition, the joint position constraints can be
replaced by
q p
j
q, j = 0 m (4.17)
in the parameter space. Similarly, the joint velocity constraints are dened by
q
k 1
t
j+k1
t
j
(p
j
p
j1
) q (4.18)
and the acceleration constraints
q (k 1)(k 2)
_
p
j
p
j1
t
j+k1
t
j
p
j1
p
j2
t
j+k2
t
j1
_
q (4.19)
where q q(t) q and q q(t) q. This can be done for the constraints on the
higher order derivatives of the joint position.
48
If we also consider a time-scale factor p
t
, which scales the time space (knot space
in our case), (4.1) becomes
q(t, p
t
, P) = q(, P) =
m
j=0
p
j
B
j,k
() (4.20)
where = p
t
t and
B
j,k
() =
_
_
g
jl
(

p
t
) if t
j+l
t =

p
t
< t
j+l+1
(l = 0 k 1)
0 if t < t
j
or t t
j+k
(4.21)
It can be veried that
B
j,k
() =
1
p
t
t
B
j,k
(t) (4.22)
by replacing by p
t
t. Moreover, for

p
t
[t
j
, t
j+k
),
B
j,k
()
p
t
=
g
jl
(

p
t
)
p
t
=
g
jl
(t)
t
(

p
t
)
p
t
=
p
2
t
B
j,k
(t)
t
=
p
t
B
j,k
()
(4.23)
Therefore, referring to (4.20) it implies that
q()
p
t
=
p
t
q()
(4.24)
Higher order derivatives of q() with respect to p
t
can be computed by a similar
procedure.
4.4 Uniform Quintic B-Spline Curves
In this dissertation, uniform quintic B-splines are used to construct the joint
trajectory in the optimization problem. A uniform quintic B-spline basis function is
B
j,6
(t) = (1)
6
(t
j+6
t
j
)[t
j
(6) : x](t x)
5
+
49
= (t
j+6
t
j
)
[t
j+1
(5) : x](t x)
5
+
[t
j
(5) : x](t x)
5
+
t
j+6
t
j
=
[t
j+2
(4) : x](t x)
5
+
[t
j+1
(4) : x](t x)
5
+
t
j+6
t
j+1
[t
j+1
(4) : x](t x)
5
+
[t
j
(4) : x](t x)
5
+
t
j+5
t
j
=
[t
j+2
(4) : x](t x)
5
+
2[t
j+1
(4) : x](t x)
5
+
+ [t
j
(4) : x](t x)
5
+
5t
=
=
1
5!
(t (j 5)t)
5
+
6(t (j 4)t)
5
+
+
15(t (j 3)t)
5
+
20(t (j 2)t)
5
+
+
15(t (j 1)t)
5
+
6(t jt)
5
+
+ (t (j + 1)t)
5
+
(4.25)
over the knot space t
j
, t
j+1
, , t
j+6
= (j 5)t, (j 4)t, , (j +1)t where
j is an integer. The joint trajectory is a quintic B-spline curve over the knot space
0 = t
0
= = t
5
t
6
=
65
m4
t
f
t
m
=
m5
m4
t
f
t
m+1
= = t
m+6
= t
f
such
that
q(t, P) =
m
j=0
p
j

B
j,6
(t) (4.26)
where P = p
0
, , p
m
are the control points, and

B
j,6
(t) = B
j,6
(t) for j = 5, 6, ,
m 5. Although the B-spline basis functions,

B
0,6
, ,

B
4,6
and

B
m4,6
, ,

B
m,6
,
have repeated knots and therefore are nonuniform, they can actually be written as
the following linear combinations of B
j,6
in (4.25) for t [0, t
f
]:
_
B
0,6
(t)
B
1,6
(t)
B
2,6
(t)
B
3,6
(t)
B
4,6
(t)
_
_
=
_
_
120 0 0 0 0
195
15
2
0 0 0
275
3

55
6
20
9
0 0
50
3
35
12

25
18
5
4
0
1
1
4
1
6

1
4
1
_
_
_
_
B
0,6
(t)
B
1,6
(t)
B
2,6
(t)
B
3,6
(t)
B
4,6
(t)
_
_
(4.27)
50
_
B
m,6
(t)
B
m1,6
(t)
B
m2,6
(t)
B
m3,6
(t)
B
m4,6
(t)
_
_
=
_
_
120 0 0 0 0
195
15
2
0 0 0
275
3

55
6
20
9
0 0
50
3
35
12

25
18
5
4
0
1
1
4
1
6

1
4
1
_
_
_
_
B
m,6
(t)
B
m1,6
(t)
B
m2,6
(t)
B
m3,6
(t)
B
m4,6
(t)
_
_
(4.28)
These equations show that the joint trajectory dened in (4.26) can be considered to
be a uniform B-spline curve. The advantage of using the uniform B-spline curve is
that it provides a possible means to generate cyclic gait motions with same end-point
conditions, q(0) = q(t
f
), q(0) = q(t
f
), and so on.
Because this B-spline has repeated knots in the beginning and the end,
B
0,5
=

B
m+1,5
=

B
0,4
=

B
1,4
=

B
m+1,4
=

B
m+2,4
= 0
in (4.13) and (4.14). The derivatives of q become
t
q(t) = 5
m
j=1
p
j
p
j1
t
j+5
t
j
B
j,5
(t) (4.29)
2
t
2
q(t) = 20
m
j=2
_
p
j
p
j1
t
j+5
t
j
p
j1
p
j2
t
j+4
t
j1
_

B
j,4
(t)
t
j+4
t
j
. (4.30)
Using (4.6), it can be shown that at t = 0, for k 6, only

B
0,6
,

B
1,5
,

B
2,4
,

B
3,3
,
B
4,2
,

B
5,1
are equal to 1 and the rest of the

B
j,k
s are equal to 0. Similarly,
at t = t
f
, for k 6, only

B
m,6
,

B
m,5
,

B
m,4
,

B
m,3
,

B
m,2
,

B
m,1
are equal to 1 and
the rest of the

B
m,k
s are equal to 0. If there were initial conditions, such as
q(0) = q
0
, q(0) = q
0
, q(0) = q
0
, then (4.26), (4.29) and (4.30) can easily be evaluated
to obtain q(0), q(0) and q(0) from the

B
j,k
s:
q(0) = p
0

B
0,6
(0) (4.31)
q(0) = 5
p
1
p
0
t
6
t
1
B
1,5
(0) (4.32)
51
q(0) = 20
_
p
2
p
1
t
7
t
2
p
1
p
0
t
6
t
1
_

B
2,4
(0)
t
6
t
2
. (4.33)
The result is
p
0
= q
0
(4.34)
p
1
p
0
=
t
5
q
0
(4.35)
p
2
3p
1
+ 2p
0
=
t
2
10
q
0
, (4.36)
where t = t
f
/(m 4). The equations for satisfying nal conditions, q(t
f
) =
q
f
, q(t
f
) = q
f
, q(t
f
) = q
f
, can be obtained in a similar procedure and are:
p
m
= q
f
(4.37)
p
m1
p
m
=
t
5
q
f
(4.38)
p
m2
3p
m1
+ 2p
m
=
t
2
10
q
f
. (4.39)
52
Chapter 5
Dynamic Motion Planning for
Open-Chained Robots
In this chapter we develop a dynamic motion planner for open-chained robots.
The joint trajectories are dened by B-spline polynomials along with a time-scale fac-
tor. The motion planning problem is transformed from an optimal control problem to
a parameter optimization one. The problem is solved by a gradient-based optimiza-
tion method in which the gradient is determined analytically. A weightlifting motion
is generated for a PUMA 762 robot whose physical limitations are incorporated into
the formulation. The torque limits are formulated as a penalty function (soft con-
straints) added into the objective function while the position and velocity limits are
formulated as linear inequalities (hard constraints). Not only does the optimal path
extend the payload capability, but also reduces the joint torques by passing through
singular congurations. A video le of the resulting motions can be found at the web
site http://www.eng.uci.edu/chwang/project/puma762.html.
53
5.1 Optimization Problems for Robots
An optimal control problem can be dened as
min
uU
J = [x(t
f
)] +
_
t
f
t
0
L(x, u)dt (5.1)
s.t. x = f(x, u) (5.2)
x(t
0
) = x
0
(5.3)
[x(t
f
)] = 0 (5.4)
where J is the cost function (or performance index) in the Bolza form; x is the state
variable belonging to the state space X '
n
; and u is the control input belonging
to the feasible controls U. Equations (5.2), (5.3) and (5.4) are the system dynamics,
initial conditions, and l constraint equations (l < n), respectively. In a robot system,
the state variable includes the joint positions and velocities, and the control input
is the applied force. The cost function can be the applied eort which is the time
integral of the joint torque squared, for example.
A gradient-based direct method is a common approach for searching for the
optimal solution to the optimal control problem. The optimal control problem is
transformed to a parameter optimization problem by expressing the control in terms
of parameters, p
i
, i = 1 m. A simple way for parameterization is the use of
polynomials, such as
u(t; P) = p
0
+p
1
t +p
2
t
2
+ +p
m
t
m
(5.5)
where p
i
P (the parameter space). The gradient
J
p
is evaluated by
J
p
=
J
x
x
p
where
x
p
is obtained by integrating the following dierential equation
d
dt
x
p
=
f
x
x
p
+
f
u
u
p
(5.6)
54
through time. Then, for instance using a steepest descent approach, the parameters
can be updated by
p
j+1
= p
j
(
J
p
)
T
(5.7)
with the stepsize . Advanced gradient based methods can be found in [34].
In the hybrid dynamics system of a robot manipulator, one can specify either
the trajectory or applied force for each joint to compute the corresponding applied
force or acceleration. Therefore, the trajectory can also be parameterized for each
joint rather than the applied force is in (5.5).
Considering the parameterized control in (5.5), the parameters, p
i
s, correspond-
ing to dierent orders have dierent inuences on the polynomial curve. The diculty
of converging to a good optimal solution can be foreseen. Hence, a parameterization
in which the parameters have equal eects on the parametric curve is desired and
may result in a faster convergence rate and even a better solution to the optimization
problem. For this reason, the composite curve consisting of a string of curve segments
(piecewise polynomials) becomes a good candidate for parameterization. Among the
choices for composite curves is the B-spline which was introduced in last chapter.
In the next section, we will introduce a method for estimating the physical
properties of a robot manipulator. Without these properties, neither accurate robot
control nor realistic robot simulation can be achieved.
55
5.2 Identication of Mass Properties
Accurate robot control and realistic robot simulation require an accurate model
of the robot manipulator. The design of robot controllers, such as computed torque,
is based on the robot model, and its performance depends on the model accuracy.
A realistic robot simulation can only be achieved with a precise dynamic model.
The dynamic model of a robot manipulator requires the physical properties of mass,
moment of inertia, center of mass, and friction. In practice, it is dicult to measure
these properties directly. Even though they can be determined by computer modeling,
they may not be accurate because of the modeling errors of the complex shapes of
the manipulator wires and other components.
Experimental system identication is one method to estimate the physical prop-
erties of a robot manipulator. It is performed o-line by collecting data needed for
estimation of the mass properties. Our approach is to form a new equation by ex-
tracting the unknown properties from the original dynamics equation (3.1)
H(q) q +h(g, q) = = Y (q, q, q)a , (5.8)
where the matrix Y depends only on q, q and q, and the vector a contains the
property information. If the joint torques, positions, velocities and accelerations can
be measured, then the physical properties can be found.
56
Recall the Newton-Euler recursive formulation of inverse dynamics in Algorithm
3.1. The spatial inertia of link i can be expressed as
J
i
=
_
_
J
ixx
J
ixy
J
ixz
0 m
i
r
iz
m
i
r
iy
J
ixy
J
iyy
J
iyz
m
i
r
iz
0 m
i
r
ix
J
ixz
J
iyz
J
izz
m
i
r
iy
m
i
r
ix
0
0 m
i
r
iz
m
i
r
iy
m
i
0 0
m
i
r
iz
0 m
i
r
ix
0 m
i
0
m
i
r
iy
m
i
r
ix
0 0 0 m
i
_
_
(5.9)
which contains ten components of the i
th
link dynamic properties. The components
can be arranged in the following vector from
a
i
= [a
i1
a
i2
a
i3
a
i4
a
i5
a
i6
a
i7
a
i8
a
i9
a
i10
]
T
= [J
ixx
J
ixy
J
ixz
J
iyy
J
iyz
J
izz
m
i
r
ix
m
i
r
iy
m
i
r
iz
m
i
]
T
.
The spatial inertia matrix in (5.9) can be written as
J
i
=
10
j=1
J
ij
a
ij
(5.10)
where J
ij
is a 6 6 matrix of zeros and ones, where the ones correspond to the
location of a
ij
. For simplicity, assume no external force applied, i.e. F
n+1
= 0.
Referring to Algorithm 3.1 the spatial force at each joint corresponding to a
i
j
= 1
can be expressed as
f
i,j
=
_
_
0 (i > i
)
J
i

V
i
ad
V
i
J
i
j
V
i
(i = i
)
Ad
T
1
i,i+1
f
i+1,j
(i < i
)
(5.11)
These spatial forces from the distal links are summed and transmitted to the prox-
imal joints. In other words, the spatial force at each joint depends linearly on the
57
physical properties. Including the friction terms such that a
fi
= [f
ci
f
vi
]
T
, and setting
f
i
= [f
i1
f
i10
] '
610
, the driving forces can be written as the following linear
equations of the physical properties a:
=
_
_
S
T
1
f
1
S
T
1
Ad
T
1
1,2
f
2
S
T
1
Ad
T
1
1,n
f
n
0 S
T
2
f
2
S
T
2
Ad
T
1
2,n
f
n
.
.
.
.
.
.
.
.
.
.
.
.
0 0 S
T
n
f
n
_
_
_
_
a
1
a
2
.
.
.
a
n
_
_
+
_
_
sgn( q
1
) q
1
0 0 0 0
0 0 sgn( q
2
) q
2
0 0
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
0 0 0 0 sgn( q
n
) q
n
_
_
_
_
a
f1
a
f2
.
.
.
a
fn
_
_
= Y
m
a
m
+Y
f
a
f
= [Y
m
Y
f
]
_
_
a
m
a
f
_
_
= Y a (5.12)
where Y
m
'
n10n
, Y
f
'
n2n
, Y '
n12n
, a
m
'
10n
, a
f
'
2n
, and a '
12n
.
Equation (5.12) represents the dynamics of the robot manipulator for one sample
point. For extra data, (5.12) is augmented as
_
_
(s
1
)
.
.
.
(s
N
)
_
_
=
_
_
Y (s
1
)
.
.
.
Y (s
N
)
_
_
a (5.13)
where s
i
denotes the i
th
sampling point and i = 1 N. Every term in (5.13) can
be measured at each sample point except the parameter vector a. Unfortunately, the
simple least squares method cannot be applied for estimation:
a
est
= (Y
T
Y )
1
Y
T
(5.14)
58
x
y
Figure 5.1: The home position of the PUMA with all the joint positions = 0.
since Y
T
Y is not invertible due to the loss of rank from restricted degrees of free-
dom at the proximal links and the lack of full force-torque sensing. Hence, a QR-
decomposition is used to resolve this problem.
5.2.1 Application: PUMA 762
The method was used to estimate the physical properties of a PUMA 762 robot.
Figure 5.1 shows the home position of the PUMA with all the joint positions equal to
zero. Table 5.1 shows the joint locations and associated rotation axes of the PUMA
where (0, 0, 0) is the intersection of Joint 1 and Joint 2 rotation axes. In this case, the
dimension of a is 72. The PUMA was controlled by a PD controller and was excited
by dierent sinusoidal trajectories at each joint. The data of corresponding torque
and position were collected. Butterworth ltered velocities and accelerations were
computed from the nite dierence derivatives of the position data. Then the dynamic
properties are estimated by using least squares on (5.12) with a QR-decomposition.
Several results from dierent sets of data were obtained. One of them is shown in
59
Location (m) Rotation Axis
Joint 1 (0, 0, 0) (0, 1, 0)
Joint 2 (0, 0, .35) (0, 0, 1)
Joint 3 (.65, 0, .19) (0, 0, 1)
Joint 4 (.65, .6, .19) (0, 1, 0)
Joint 5 (.65, .6, .19) (0, 0, 1)
Joint 6 (.65, .6, .19) (0, 1, 0)
Table 5.1: The joint locations and associated rotation axes of the PUMA.
Table 5.2. However, some of the parameters are not identiable, i.e. most of the zero
terms in Table 5.2, due to the restricted motion of proximal links and the lack of
full force/torque sensing. For example, because the PUMA has only revolute joints,
the absence of the translation motion results in the zero estimated mass terms. In
addition, some of the estimated friction values are negative at the last three joints of
the PUMA due to the complicated gear interaction between joints and motors. (The
dynamics of gears and motors were not taken into account, though the interaction
was considered.)
Physically, mass is nonzero and friction is opposite to the moving direction
(positive in our case). Therefore, we pre-specied the link masses and joint frictions
by estimation based on the PUMAs physical dimensions and observing the dierent
results from dierent sets of data, respectively. It leaves nine parameters for each
link instead of twelve. The obtained properties is shown in Table 5.3. The computed
dynamic response from the estimated properties was nearly identical to the response
of the real Puma for dierent motions.
60
Parameters I
xx
I
xy
I
xz
I
yy
I
yz
I
zz
Link 1 0.00 0.00 0.00 86.64 0.00 0.00
Link 2 47.68 2.20 1.75 0.00 1.31 58.09
Link 3 0.00 2.44 1.83 0.00 3.41 12.98
Link 4 0.00 0.32 0.21 0.18 0.15 0.68
Link 5 0.10 0.21 0.02 0.00 0.47 0.00
Link 6 0.40 0.04 0.19 0.09 0.06 0.24
Parameters mr
x
mr
y
mr
z
m f
c
f
v
Link 1 0.00 0.00 0.00 0.00 0.1692 0.0049
Link 2 5.79 5.66 0.00 0.00 0.4135 0.0011
Link 3 0.05 8.62 0.00 0.00 0.4218 0.0006
Link 4 0.34 0.05 0.00 0.00 0.2687 0.0065
Link 5 0.23 0.88 0.22 0.00 0.1635 0.0002
Link 6 0.00 0.00 0.02 0.00 0.0766 0.0017
Table 5.2: The physical properties without any prediction of mass and friction
The system identication method is similar to the method found in the work of
An et al. [3], but was done independently. Our method uses the Lie group formulation
to derive a systematic matrix form of the dynamics equations. Friction is taken into
account in the system. A lter was used on the sensed torques and positions in our
method.
61
Parameters I
xx
I
xy
I
xz
I
yy
I
yz
I
zz
Link 1 0.00 0.00 0.00 4.63 0.00 0.00
Link 2 4.42 2.40 9.39 0.00 1.52 13.98
Link 3 0.00 2.19 2.03 11.99 3.31 0.00
Link 4 0.00 0.36 0.20 0.19 0.12 0.78
Link 5 0.00 0.22 0.06 0.00 0.44 0.04
Link 6 0.40 0.03 0.19 0.08 0.08 0.23
Parameters mr
x
mr
y
mr
z
m f
c
f
v
Link 1 0.00 0.00 0.00 50.00 0.3000 0.0025
Link 2 73.35 5.64 0.00 180.00 0.4800 0.0007
Link 3 0.07 6.63 0.00 90.00 0.3900 0.0011
Link 4 0.38 6.73 0.19 5.00 0.0800 0.0012
Link 5 0.22 0.87 0.00 6.00 0.1100 0.0010
Link 6 0.01 0.00 0.04 3.00 0.0800 0.0018
Table 5.3: The physical properties with prediction of mass and friction
5.3 Weightlifting Motion Planning
Consider an n degree of freedom (DOF) open chain manipulator. The optimiza-
tion problem for this system is:
min J
c
= J
w
+J
e
(5.15)
s.t. H(q) q +h(q, q) = (5.16)
(t) (5.17)
q q(t) q (5.18)
62
q q(t) q (5.19)
q(0) = q
0
, q(0) = 0 (5.20)
q(p
t
t
f
) = q
f
, q(p
t
t
f
) = 0 (5.21)
with J
w
= w p
w
(5.22)
J
e
=
1
2
_
p
t
t
f
0
T
W
e
dt (5.23)
Equation (5.16) represents the dynamics for the open chain manipulator with the
joint coordinates q '
n
and the joint forces or torques '
n
, where H(q) is the
mass or inertia matrix and h(q, q) contains the centrifugal, Coriolis and gravitational
forces and friction. Constraints on the joint torque , position (displacement) q and
velocity q are represented by (5.17), (5.18), (5.19), respectively, where , , q, q, q
and q '
n
are assumed to be given for a particular manipulator. The weight carried
by the end-eector is p
w
.
The optimization problem is a mixed problem with parameter optimization
and with optimal control. The cost J
w
denotes the parameter optimization which
maximizes the payload p
w
lifted by the manipulator while J
e
denotes the optimal
control which minimizes the eort applied, where w and W
e
denote the positive
weighting factors with appropriate dimensions. It is a xed-nal-time problem if the
nal time t
f
is chosen and the time-scale factor p
t
is set to be one. On the other
hand, a free-nal-time problem can be established by xing t
f
and varying p
t
as an
additional parameter.
Matlabs BFGS SQP algorithm, constr, is used to search for the solution of the
optimization problem. Unfortunately, for problems that are ill-conditioned to begin
with, approximate nite dierence gradients can lead an unbounded condition number
of the approximate Hessian of J
c
, and the algorithm will fail. Therefore, the method
63
discussed in last chapter is required to compute not only the dynamic equation (5.16)
but also its derivatives with respect to the parameters in the problem.
Parameterizing the joint trajectories with the quintic B-spline, q = q(t, p
t
, P), in
(4.26) and applying the results in Section 4.4, the optimization problem is transformed
in the following form:
min
p
w
,p
t
,P
J
c
= J
w
+J
e
+J
p
(5.24)
s.t. H(q) q +h(q, q) = (5.25)
q p
j
q (5.26)
q
(k 1)(p
j
p
j1
)
t
j+k1
t
j
q (5.27)
p
0
= p
1
= q
0
, p
m1
= p
m
= q
f
(5.28)
with J
p
=
1
2
_
p
t
t
f
0
T
+
W
p
+
d (5.29)
+
=
_
_
if > 0
0 otherwise
(5.30)
=
_
_

_
_
(5.31)
where P = p
0
, , p
m
with p
j
'
6
is the set of the control points; and J
p
is a
penalty function with a positive weighting factor W
p
bounding within [ ].
The gradient of the cost J
c
is:
J
c
=
_
J
c
P
J
c
p
t
J
c
p
w
_
(5.32)
which can be computed from the analytical results in the previous sections. Note
that friction is not taken into account in the optimization since Coulomb friction is
not continuously dierentiable.
64
5.3.1 Optimization Results
In this section, we apply the optimization approach to the PUMA 762 robot
whose mass properties were estimated by system identication described in Section
5.2.1. The optimization problem formulated in the last section was used to improve
the PUMA payload capability. The manufacturers specications state that a maxi-
mum load of 20 kg can be lifted at a distance of 25 cm from the center of its wrist. As
mentioned previously, the payload was modeled as an uniform disc. The trajectories
of the Pumas six joints were parameterized with uniform quintic B-splines.
We tested the algorithm for three dierent cases of 1 DOF, 3 DOFs and 6 DOFs
as discussed below. The goal in all cases was to move the payload from the downward
posture shown in Figure 5.2, to the upward posture. However, dierent numbers of
degrees of freedom were used in each case to execute the motion. The cases were:
1 DOF: the 4
th
joint is the only joint with freedom to move;
3 DOF: the last three joints (wrist) are set free while the others are xed; and
6 DOF: all six joints are free to move.
In each case, the optimization procedure was initialized with p
w
= 20 kg, p
t
= 1, t
f
=
20 sec and a prescribed path which moves the 4
th
joint from the initial conguration
to the nal one, shown in Figure 5.2, on a smooth trajectory without any oscillation.
The other joints were initially parameterized to be constant functions of time during
the motion they did not move initially. The solutions to the optimization problem
for the three cases are shown in Table 5.4, and Figures 5.35.5. Table 5.4 shows the
initial and nal values of the cost function, along with the path traversal time and
65
1 0.8 0.6 0.4 0.2 0 0.2 0.4 0.6 0.8 1
1
0.8
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
1 0.8 0.6 0.4 0.2 0 0.2 0.4 0.6 0.8 1
1
0.8
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
Figure 5.2: The initial (left) and nal (right) congurations of the puma.
the payload for all three cases. Note for the 1 DOF and the 3 DOF cases, the motors
that were held xed needed to exert torque on the robot during the motion, but this
cost was not included in the evaluation of J
cp
.
For the 1 DOF case, Figure 5.3 shows that energy is pumped into joint 4 through
a swinging motion. The amplitude of the swing increases until the velocity of joint
4 reaches its limit. At this point, the arm continues to move until the nal upward
posture is reached. A comparison of the joint velocity and applied torque shows that
both have the same sign throughout most of the time interval. This is expected
since energy must be added to the system in order to swing up the weight. One
could conjecture that minimal eort motions could thus be constructed by using the
natural oscillations of the system and applying a small torque in phase with the
velocity (i.e. switching the torque when the velocity changes sign). However the
real physical constraints, such as the velocity bounds, would make it dicult to plan
motions in this manner that satisfy the boundary conditions on the motion. Note
that semi-innite velocity bound is an active constraint for almost an entire second
during the motion.
66
Case Initial J
c
Final J
c
Final Time Payload
1 DOF 24.2 42.5 7.8 sec 29.4 kg
3 DOF 4.15 21.1 3.1 sec 46.0 kg
6 DOF 181.0 600.1 11.9 sec 63.2 kg
Table 5.4: The optimal results for three cases
When this trajectory was experimentally tested on the Puma, we rst attempted
to move the arm along the initial trajectory with no swings, but with the larger
payload. This indeed caused the amplier for joint 4 to overload and automatically
shut down, with the brakes applied. We then tested the optimal swinging trajectory,
and found that only about 25 kg, rather than the predicted 29.4 kg, could actually
be lifted without causing the amplier to overload. We believe that the dierence is
due to imprecise knowledge of the actual bounds for torque and velocity. The torque
limits,
max
, were found experimentally from static measurements of the stall torque
for each motor, when q = 0. In order to account for nonzero q we made = .75
max
.
In addition, we also made q = .8 q
max
, where q
max
is the velocity limits specied in
the Puma manuals. Other, more complicated bounds could easily have been used in
our algorithm if they were known.
The motion shown in Figure 5.4 is the 3 DOF case where the entire wrist is
allowed to move. Although it is not obvious from the gure (the actual motions can
be seen at the website http://www.eng.uci.edu/chwang/project/puma762.html
or in the video proceedings of the 1999 ICRA), the wrist moves in a spherical os-
cillation using all three joints until the payload axis has lifted 90
o
from the starting
conguration. At this point the wrist is in a singular conguration which requires no
torque from the motors of the wrist to keep the weight suspended. Then the payload
67
0 1 2 3 4 5 6 7
5
4
3
2
1
0
1
2
3
4
5
q
4

(
r
a
d
)
t (sec)
0 1 2 3 4 5 6 7
2.5
2
1.5
1
0.5
0
0.5
1
1.5
2
2.5
v
4

(
r
a
d
/
s
e
c
)
t (sec)
0 1 2 3 4 5 6 7
2
1.5
1
0.5
0
0.5
1
1.5
2
4

(
N
m
)
t (sec)
Figure 5.3: The motion, speed and torque of the 4
th
joint in the 1 DOF case. (Limits:
dotted lines)
continues moving upwards until it reaches the desired nal conguration. Using all
three joints in this manner, the payload was increased to 46 kg.
The nal motion, shown in Figure 5.5, is the 6 DOF case where all the joints
were allowed to move. In this case, the actual motion (rather than the time-sequence)
is very dramatic since the potential energy of the payload is rst transformed into
internal kinetic energy of the robot and then, through some interesting maneuvering,
it arrives at its desired nal conguration. The payload actually spins signicantly
about its axis during the motion, using gyroscopic eects to assist in the weightlifting.
68
Figure 5.4: The motion of the wrist in the 3 DOF case.
For this case, the payload was increased over the previous cases to 63 kg, or more
than triple the manufacturers specications.
5.3.2 Discussion
In order to reduce the motor torques, the gear ratios and the gear interaction
between the motors and the joints must be taken into account in the problem formu-
lation. The linear relation between the motor displacements and joint displacements
are represented as a 6 6 matrix B such that q = B q
m
, where q
m
is the motor shaft
velocity. Applying virtual work, we can describe the motor torque
m
in terms of the
joint torque as follows:
T
q =
T
m
q
m

m
= B
T
. (5.33)
69
Figure 5.5: The motion for the 6 DOF case.
In addition, in the Puma, the maximum motor torques for the rst three joints is
about 5 times those for the wrist joints. Therefore, the weighting factors, W
e
and
W
p
, are set as:
W
e
= c
e
W
b
and W
p
= c
p
_
_
W
b
0
0 W
b
_
_
with
W
b
= B
_
_
1
5
1
33
0
0 1
33
_
_
B
T
(5.34)
where c
e
and c
p
are scalar weighting factors; 1 is an identity matrix; and
1
5
is replaced
by 0 in the 1 DOF and 3 DOF cases.
In the optimization problem, it is necessary to choose appropriate weighting
factors, w, c
e
and c
p
. Ideally one would make the joint torque penalty term c
p
as
large as possible in order to bound the joint torques within their limits. However,
70
as is well-known with penalty function methods, the optimization problem becomes
ill-conditioned and will not converge if c
p
becomes too large. In addition, we expect
that a small value for w and a large value for c
e
will yield a solution with a small
payload, while a large w and small c
e
will yield a large payload. This was consistent
with our optimization results. Typical values used for our results were c
e
= 1 and
c
p
= 10000. The value used for w was 30, 10, and 200 for the 1 DOF, 3 DOF, and
6 DOF cases, respectively.
All of the results obtained are local minimizers of the nonlinear cost function.
It is desirable to obtain globally minimizing trajectories. We started the algorithm
with dierent initial paths in a search for other minimizing trajectories. We found
several other interesting local minimizers for the 6 DOF case, which can be seen in
the video proceedings of the 1999 ICRA. The highest payload was achieved with the
motion shown in Figure 5.5. In all the cases, we rst solved the optimization with
a low number of spline parameters m = 10 in (4.26). We then increased m to 15,
and used the solution for m = 10 as the starting point for a new optimization. In
most cases the trajectory did not change signicantly. If it did, we further increased
m. The nal value for m for the 1 DOF case was m = 25, and, for the 6 DOF
solution shown, the nal value was m = 15. Note that for the 6 DOF case, m = 15
means that there were a total of 6(m 3) + 2 = 74 variable parameters used in the
optimization. This includes the time scale parameter and the payload parameter.
The computation time for the optimization ranged from several minutes to about an
hour on a Pentium II-300 PC. The time depended on the number of parameters used,
and on the termination tolerance of the optimization.
As mentioned previously, there is no exact mathematical model that gives the
peak torque versus velocity relation for the motor ampliers of the Puma. Although
71
we restricted the velocity limits to 80% of those specied in the Puma documents, and
the torque bounds to 75% of their static bounds, we found several motions that caused
the ampliers to overload when tested on the real system. It was also interesting that
higher payloads could be achieved when the Puma was cold than could be obtained
after it had been in operation for some timeprobably due to the thermal overload
switches in the ampliers.
Finally, it is interesting to note that the resulting motions from our algorithm
routinely swing through singular congurations. In these congurations the actuator
eort is reduced since loads are supported by the robot structure. Note that humans
also move to singular congurations to support large loads. However it is not known
what, if any, the cost function is that they are minimizing. There is some clinical
evidence that humans may minimize jerk or [29].
5.3.3 Conclusions
We developed a weightlifting motion planner for robots which combines maxi-
mization of the payload with minimization of the eort. Our solutions demonstrated
that optimal motions routinely swing through singular congurations. As is often
done by human weightlifters, our solutions also often used a pumping motion to as-
sist the lift. The approach can be applied to any manipulation task where payloads
need to exceed the manufacturers limitsthe main requirement is that the robot can
support the weight statically in the initial and nal congurations.
In our approach, the optimal control problem is re-formulated as a parameter
optimization with variable control points for the B-splines, a time-scale factor, and
an unknown payload. The dierentiability of the B-splines together with the limited
72
support property provide the ability to calculate derivatives with respect to the control
points and the time-scale factor. In addition, based on the convex hull property of
the B-splines, the semi-innite constraints on the joint position and velocity can be
transformed to linear inequalities in the parameter space.
73
Chapter 6
Dynamic Motion Planning for the
Design of Robotic Gait
Rehabilitation
In this chapter we examine a method to control the stepping motion of a par-
alyzed person suspended on a treadmill using a robot attached to the pelvis. A leg
swing motion is created by moving the pelvis without contact with the legs. The
problem is formulated as an optimal control problem for an underactuated artic-
ulated chain. The optimal control problem is converted into a discrete parameter
optimization and an ecient gradient-based algorithm is used to solve it. Motion
capture data from an unimpaired human subject is compared to the simulation re-
sults from the dynamic motion optimization. Our results suggest that it is feasible to
create a gait for a paralyzed person that is close to that of an unimpaired subject by
controlling the pelvis with a robot. The resulting motions can be found at the web
site http://www.eng.uci.edu/chwang/project/stepper/stepper.html.
6.1 Introduction
In the U.S. alone, over 700,000 people experience a stroke each year, and over
10,000 people experience a traumatic spinal cord injury. Impairment in walking ability
74
Figure 6.1: Body weight supported (BWS) step training following spinal cord injury.
Two therapists assist in leg movement, while a third assists in torso movement. From
Behrman and Harkema (2000).
after such neurologic injuries is common. Recently, a new approach to locomotion
rehabilitation called body weight supported (BWS) training has shown promise in
improving locomotion after stroke and spinal cord injury [62]. The technique involves
suspending the patient in a harness above a treadmill in order to partially relieve the
weight of the body, and manually assisting the legs and pelvis in moving in a walking
pattern (Figure 6.1). Patients who receive this therapy can signicantly increase their
independent walking ability and overground walking speed [5]. It is hypothesized
that the technique works in part by stimulating remaining force, position, and touch
sensors in the legs during stepping in a repetitive manner, and that residual circuits in
the nervous system learn from this sensor input to generate motor output appropriate
for stepping.
Clinical access to BWS training is currently limited because the training is labor
intensive. Multiple therapists are often required to control the pelvis and legs. Several
75
research groups are pursuing robotic implementations of BWS training in an attempt
to make the training less labor intensive, more consistent, and more widely accessible
[8, 27, 50, 17]. Implementing BWS training with robotics is also attractive because
it could improve experimental control over the training, thus providing a means to
better understand and optimize its eects.
A diculty in automating BWS training is that the required patterns of forces
at the pelvis and legs are unknown. For example, the relative importance of assisting
at the pelvis and legs is unclear. One approach toward determining the required
forces is to instrument the therapists hands with force and position transducers [8].
However, therapists are relatively limited in the forces that they can apply compared
to robots, and there is no guarantee that any given therapist has selected an optimal
solution. Current robotic devices for BWS have taken the simplied approach of
replicating leg kinematics, and moving the torso in a gait, like up and down pattern
[17, 26].
In this chapter we explore an alternate approach toward generating strategies
for assisting in gait training: dynamic motion optimization. Dynamic motion opti-
mization provides a formalized method for determining motions for underconstrained
tasks, and may reveal novel strategies for achieving the tasks. It has been used with
success to simulate human control over such activities as diving, jumping, and walking
[2, 28, 40, 4]. The objective of this study was to use dynamic motion optimization to
determine to what extent repetitive stepping by a paralyzed patient can theoretically
be generated by assisting in pelvis motion alone.
76
100 50 0 50 100
0
20
40
60
80
100
120
140
160
x (cm)
y

(
c
m
)
Figure 6.2: One-half of the gait cycle (duration 0.467 sec).
6.2 Methods
6.2.1 Human Model and Walking Motion
For studying the motion of the legs, the head, torso, pelvis, and arms were
combined into a single rigid body referred to as the upper trunk. The walking gait
cycle was assumed to be bilaterally symmetric; that is, the left-side stance and swing
phases were assumed to be identical to the right-side stance and swing phases, re-
spectively. Based on this assumption only one-half of the gait cycle was simulated.
We refer to the joints on the side of the stance phase as the stance joints and the
joints on the side of the swing phase as the swing joints. The stance hip was mod-
eled as a 2 degrees-of-freedom (DOF) universal joint rotating about axes oriented in
the x- and y-directions (refer to Figure 6.2). These were the DOF assumed to be
controlled by a robotic device. The upper trunk was assumed to remain at a xed
angle about the z-axis. The swing hip was modeled as a 3 DOF ball joint rotating
77
l
Hip
l
ULeg
l
LLeg
l
Foot
l
Toe
l
Heel
0.146 m 0.470 m 0.489 m 0.079 m 0.175 m 0.069 m
Table 6.1: Link lengths
about axes in the x- (hip adduction/abduction), y- (hip internal/external rotation),
and z- (hip exion/extension) directions. The knee and ankle were modeled as 1
DOF hinge joints about the z-axis (knee extension/exion and ankle dorsal/plantar
exion, respectively).
Motion capture data of key body segments for an unimpaired subject during
treadmill walking was obtained using a video-based system (Motion Analysis Corp.)
at the Department of Veterans Aairs Medical Center, Long Beach, CA. The sampling
rate of motion capture was 60 Hz. External markers were attached to the subject
at the antero-superior iliac spines (ASISs), knees, ankles, tops of the toes, and backs
of the heels. A least squares method was used to convert the positions of markers
to the link lengths and joint angles based on forward kinematics of the dynamic
model described above. The detailed procedure is described in Appendix A. The
resulting link lengths and joint orientations are shown in Table 6.1 and are illustrated
in Figure 6.2 which shows one-half of the gait cycle (duration 0.467 sec). The distance
between the right and left hip joint center locations is l
Hip
; and the lengths of upper
and lower legs l
ULeg
and l
LLeg
, respectively. Assuming the subject stands straight up,
l
Foot
represents the vertical distance between the ankle and sole of the foot (ground);
l
Toe
the horizontal distance between the ankle and toes; and l
Heel
the horizontal
distance between the ankle and heel. Finally, when generating the motion capture
data, the stance foot moved with the belt on the treadmill while the swing leg moved
78
in the air. For a clearer visualization of the swing leg movement, a constant-velocity
transformation was applied to keep the stance foot xed in the plots
The human subject was 1.95 m tall and weighed 75 kg. Dynamic properties
of the body segments were estimated using regression equations based on segment
kinematic measurement [63]. The detailed procedure is described in Appendix B and
the results are listed in Table 6.2.
Link Mass Inertia Center of Mass
(kg) (kg m
2
) (m)
Upper Trunk 46.0704
_
_
3.2288 0 0
0 0.7783 0
0 0 2.7573
_
_
_
_
0.0009
0.3600
0
_
_
Upper Leg 9.5368
_
_
0.1574 0 0
0 0.0353 0
0 0 0.1535
_
_
_
_
0.0240
0.1700
0.0070
_
_
Lower Leg 3.5586
_
_
0.0638 0 0
0 0.0055 0
0 0 0.0624
_
_
_
_
0.0050
0.2073
0.0190
_
_
Foot 1.4373
_
_
0.0028 0 0
0 0.0088 0
0 0 0.0070
_
_
_
_
0.0440
0.0394
0.0090
_
_
Table 6.2: Dynamic properties of the human model (in the SI system of units)
Passive torque-angle properties of the hip, knee, and ankle joints were measured
for the subject with a Biodex active dynamometer. The dynamometer imposed slow
79
isovelocity movements at the joints and measured applied torques and resulting joint
angles. Joints were measured in a gravity-eliminated conguration, or, if not possible,
torques due to gravity were estimated and subtracted. We modeled the joints as
nonlinear springs in which the joint torque was a polynomial function of the joint
angle. A least squares method was used to obtain the best-t polynomial of order
3 for the torque-angle property of each of the joints except for the ankle joint. A
polynomial of order 7 provided a better t to the ankle joint data. The resulting
polynomial curves are listed as follows. (The joint lower and upper bounds, q and q,
are shown in the parentheses (q, q).)
Hip External/Internal Rotation (60
, 60
m
= 0.6837 0.7621q + 0.9772q
2
2.2620q
3
Hip Abduction/Adduction (50
, 30
m
= 0.0542 0.8266q 6.0205q
2
29.0271q
3
Hip Extension/Flexion (35
, 70
m
= 1.0863 + 1.5721q + 6.3488q
2
23.0405q
3
Knee Flexion/Extension (140
, 0
m
= 24.9343 53.1584q 37.5211q
2
9.8685q
3
Ankle Plantar/Dorsal Flexion (52
, 46
m
= 0.1305 3.9956q + 1.5596q
2
4.7881q
3
+2.4229q
4
+ 6.2372q
5
5.6802q
6
19.5304q
7
80
In addition to the polynomial function, a nonlinear spring-damper system is
used to place a hard limit on joint movement when it is close to its upper and lower
bounds.
sd
=
_
_
(10
4
(q q) + 5 10
2
q) if q q
(10
4
(q q) + 5 10
2
q) if q q
0 otherwise
where
=
_
_
6 10
5
(q q)
5
1.5 10
5
(q q)
4
+ 10
4
(q q)
3
if q + 0.1 q q
6 10
5
(q q)
5
1.5 10
5
(q q)
4
10
4
(q q)
3
if q 0.1 q q
1 otherwise.
The function
sd
is C
2
continuous in order to be used in the computation of the
analytical gradient in the dynamic motion optimization. The results,
msd
=
m
+
sd
with q = 0, are shown as a solid line crossing the experimental data in Figure 6.3.
Four steps at three dierent treadmill walking speeds (1.8, 1.25 and 0.77 m/sec)
were obtained from motion capture. The steps had durations of 0.43, 0.47, 0.5 and
0.57 sec. They are shown in Figures 6.46.7. The front view of the gait displays only
the nal conguration while the top view displays only the movement of the pelvis.
These data will be used for comparison with the optimization results in the following
sections.
6.2.2 Formulation of the Optimal Control Problem
The objective of this study was to explore to what extent repetitive stepping
by a paralyzed patient can theoretically be generated by moving the pelvis along
some trajectory, without explicitly controlling the legs. That is, could a normal gait
81
60 40 20 0 20 40 60 80
10
5
0
5
10
Hip External/Internal Rotation (deg)
T
o
r
q
u
e

(
N
m
)
60 40 20 0 20 40
20
10
0
10
20
30
Hip Abduction/Adduction (deg)
T
o
r
q
u
e

(
N
m
)
60 40 20 0 20 40 60 80
40
30
20
10
0
10
20
Hip Extension/Flexion (deg)
T
o
r
q
u
e

(
N
m
)
150 100 50 0 50
40
30
20
10
0
10
20
30
40
Knee Flexion/Extension (deg)
T
o
r
q
u
e

(
N
m
)
60 40 20 0 20 40 60
20
15
10
5
0
5
10
15
20
Ankle Plantar/Dorsal Flexion (deg)
T
o
r
q
u
e

(
N
m
)
Figure 6.3: Muscle system.
82
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
83
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
84
Joint Stance Hip Swing Hip Knee Ankle
DOF 3 2 3 1 1
Screw Axis S
tx
, S
ty
, S
tz
S
ry
, S
rx
S
rx
, S
ry
, S
rz
S
rz
S
rz
Joint Position q
1
, q
2
, q
3
q
4
, q
5
q
6
, q
7
, q
8
q
9
q
10
Fully Actuated Specied Path Active Active Active Active
Underactuated Specied Path Active Passive Passive Passive
Table 6.3: DOF of the human model.
be generated by a robot that maneuvers the pelvis without requiring robots that
maneuver the legs? We assumed that the robot was capable of moving the stance hip
along a normal, unimpaired trajectory, and simultaneously lifting the swing hip to
control movement of the swing leg. As a rst approximation, we modeled the impaired
leg as a paralyzed (i.e. unactuated) linkage with the identied passive torque-angle
properties.
This problem can be addressed mathematically as an optimal control problem
for an underactuated system. We are interested in obtaining a normal swing phase
of the paralyzed leg by shifting the pelvis. We used the motion of the stance hip
found from the video capture data of the unimpaired subject as an input to our
underactuated human model. The degrees of freedom of the human model are shown
in Table 6.3. S
rx
, S
ry
and S
rz
represent the screw axes of rotation about the x-, y-
and z-axes, respectively; S
tx
, S
ty
and S
tz
the screw axes of translation along the x-,
y- and z-axes, respectively; and the joint positions q
i
s are corresponding to the screw
axes sequentially. We considered the swing motion to be an optimal control problem
85
as follows:
min
(t)
J
c
=
1
2
_
t
f
0
10
i=4
w
ei
2
i
dt (6.1)
s.t. H(q) q +h(q, q) = +
msd
(6.2)
q(0) = q
0
, q(0) = q
0
(6.3)
q(t
f
) = q
f
, q(t
f
) = q
f
(6.4)
where
1
,
2
, and
3
are the generalized forces associated with the translation of
the stance hip (and are not included in the cost function since the position of the
stance hip was specied by the motion capture data);
4
and
5
are the moments
corresponding to the two rotations of the stance hip (controlled by the robot);
6
,
7
and
8
are the swing hip moments (corresponding to hip abduction/adduction
and extension/exion, external/internal rotation, respectively);
9
and
10
correspond
to knee and ankle rotation moments, respectively; and w
ei
s are positive weighting
coecients.
6

10
were assumed zero for the impaired leg.
msd4

msd10
were
modeled as nonlinear spring-damper muscle systems while
msd1

msd3
were zero
since no muscular force is needed for the linear translation of the stance hip . Equation
(6.2) represents the dynamics for the human model with the joint coordinates q
'
10
, the joint forces or torques '
10
, and the muscular forces or torques
msd

'
10
, where H(q) is the generalized mass matrix and h(q, q) contains the centrifugal,
Coriolis and gravitational forces.
In addition, it was necessary to avoid collision of the swing leg with the stance
leg and the ground. This was achieved by introducing two penalty functions into the
cost function which penalized the penetration of the swing leg with the stance leg and
the ground. Therefore, since the position of the stance hip in the z-direction is less
than that of the swing hip in the selected coordinate system (Fig 6.4), the following
86
penalty functions, J
p1
and J
p2
, were used to represent collision and were added to the
cost function:
J
p1
=
n
t
i=0
w
p1
(y
ground
y
heel
(
i
n
t
t
f
))
2
+
+ (y
ground
y
toe
(
i
n
t
t
f
))
2
+
(6.5)
J
p2
=
n
t
i=0
w
p2
(z
1
(
i
n
t
t
f
) z
knee
(
i
n
t
t
f
))
2
+
+ (z
2
(
i
n
t
t
f
) z
heel
(
i
n
t
t
f
))
2
+
(6.6)
+
=
_
_
if > 0
0 otherwise
(6.7)
where n
t
> 0 is the number of time instances when the collision is checked, and is
set to be 50 in this dissertation; w
p1
and w
p2
are positive weighting coecients; and
(x, y, z)
knee
, (x, y, z)
heel
and (x, y, z)
toe
the Cartesian coordinates of the swing knee,
heel and forefoot, respectively, which were computed using the forward kinematics.
y
ground
is the position of the ground in the y- direction which is equal to that of the
swing forefoot in the beginning of the one-half gait cycle. z
1
and z
2
are selected to
constrain the movement of the swing leg in the z-direction to avoid excessive out of
plane motion.
The following cost function was used to drive the passive joints of the swing leg
to the desired nal conguration:
J
p3
=
1
2
ip
w
p31
(q
ip
(t
f
) q
fip
)
2
+w
p32
( q
ip
(t
f
) q
fip
)
2
(6.8)
where ip is the passive joint index; q
fip
and q
fip
the nal joint position and velocity,
respectively; and w
p31
and w
p32
the weighting coecients.
87
6.2.3 Dynamic Motion Optimization via Direct Parameter
Optimization
In order to formulate the optimal control problem for a numerical solution,
the joint trajectories were interpolated by uniform, C
4
continuous quintic B-spline
polynomials over the knot space of an ordered time sequence.
Let the knot sequence be 0 = t
0
= = t
5
t
6
t
m
t
m+1
= =
t
m+6
= t
f
with m 5 and t = t
j
t
j1
=
tf
m4
for j = 6, 7, , m + 1. The joint
trajectories, q '
n
a
, are then written as
q(t, P) =
m
j=0
p
j
B
j,6
(t) (6.9)
where n
a
is the number of actuated joints; P = p
0
, , p
m
with p
j
'
n
a
is the
set of the control points; and B
j,6
is a quintic B-spline basis function discussed in
Chapter 4.
For the simulation of the paralyzed patient the system is modeled as an under-
actuated one with active and passive joints. The dynamics of such a hybrid dynamic
system can be solved eciently by the Lie group formulation discussed in Chapter 3.
In order to perform the optimization, an initial trajectory was required. We used the
trajectory identied from motion capture as the initial trajectory and dened it with
the parameter set P such that q = q(t, P).
The optimal control problem (6.1) was then transformed into the following dis-
crete parameter optimization:
min
P
J
cp
= J
c
+J
p1
+J
p2
+J
p3
(6.10)
s.t. H(q) q +h(q, q) = +
msd
(6.11)
88
p
0
= q
0
, p
1
= q
0
+
1
5
q
0
t (6.12)
p
m
= q
f
, p
m1
= q
f

1
5
q
f
t (6.13)
Note that the trajectory of the stance hip, (q
1
, q
2
, q
3
), is approximated as a B-spline
curve based on the motion capture data. Equations (6.12) and (6.13) were used to
meet the initial and nal conditions (6.3) and (6.4), respectively. The actual variable
parameters were P, excluding the xed p
0
, p
1
, p
m1
, and p
m
; and the total number
of variable parameters was n
a
(m3).
As described next, motions were generated by this dynamic motion optimiza-
tion with dierent weighting coecients for dierent cases. In each case, 8 variable
parameters (m = 11) were used for each of the active joints. The joint torques were
computed for the human model based on the estimated dynamic properties and the
B-spline joint trajectories.
6.3 Results
Using the dynamic motion optimization technique, four dierent cases were
studied:
Case 1: paralyzed swing leg with motion captured stance hip orientation (no op-
timization). This case was studied to determine if simply applying a normative
pelvis trajectory would eectively control the swing leg.
Case 2: unimpaired swing leg with eort minimization of all joints. This case
was studied to determine if the optimization technique produced a realistic gait
trajectory if the leg was fully actuated.
89
Case 3: paralyzed swing leg with eort minimization of the stance hip torques.
This case was studied to determine to what extent swing of a paralyzed leg
could be controlled with pelvis motion.
Case 4: paralyzed swing leg with eort minimization of the stance hip torques
and bounded stance hip orientation. Case 3 produced large motions of the hip.
Therefore, we sought to determine how limiting the orientation eected the
quality of swing control.
For these simulation, the stance and swing legs were the left and right legs, respec-
tively. The stance hip joint center locations for dierent treadmill walking speeds
were approximated by B-spline curves based on the motion capture data. Dierent
weighting coecients and dierent z
1
and z
2
for avoiding the collision of the legs were
used in the optimization. The results corresponding to the dierent treadmill walk-
ing speeds for each case are discussed and shown in the gures of Appendices CF,
except for the results corresponding to the walking speed with duration of 0.5 sec, i.e.
the moderate treadmill speed, shown in the following subsections. The optimization
results are represented as solid lines, and the actual human gait as dashed lines for
comparison. In the gures showing the gait, a transformation was used to keep the
stance foot xed, instead of moving at a constant speed with the treadmill. The front
view of the gait displays only the nal conguration while the top view shows only
the movement of the pelvis.
90
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
Figure 6.8: The resulting gait (duration 0.5 sec) for Case 1: Paralyzed Swing Leg
with Motion Captured Stance Hip Orientation (No Optimization). The solid lines
show the resulting gait and the dashed lines are the gait recorded from the motion
capture system.)
6.3.1 Case 1: Paralyzed Swing Leg with Motion Captured
Stance Hip Orientation (No Optimization)
No optimization was applied in this case, in order to determine how the leg
would swing if the pelvis were simply moved in a normative trajectory. The swing
hip, knee and ankle joints were set to be passive while the stance hip joint followed
the trajectory identied from motion capture. Assuming no ground contact, the
equations of motion were solved and the resulting motions are shown in Figure 6.8
and Appendix C. Figure 6.8 shows that the swing leg moves at its natural frequency,
and the nal conguration is far from the desired one. However, there would be a
collision between the leg and the ground at about x = 0 (mid-stride) if the ground
91
w
e4
, w
e5
, , w
e8
w
e9
w
e10
w
p1
w
p2
0.05 0.1 2.5 5 10
5
5 10
4
Table 6.4: Weighting coecients in Case 2: Unimpaired Swing Leg
were not neglected. Note also that the leg is internally rotated away from the desired
conguration at the end of swing.
6.3.2 Case 2: Unimpaired Swing Leg with Eort Minimization
of All Joints
In order to test the ability of the optimization technique to mimic normal human
control, we studied a fully actuated human model with active hip and knee joints in
the swing leg. 56 parameters (8 for each active joint) were used in the optimization.
The parameters that set the limits on allowable out of plane motion of the legs, z
1
and z
2
in the penalty function J
p2
, were chosen as
z
1
(t) = z
stance hip
(t) +l
hip
0.005 (6.14)
z
2
(t) = z
stance hip
(t) +l
hip
0.033 (6.15)
where l
hip
0.005 and l
hip
0.033 are the smallest distances between the swing
knee and the stance hip and between the swing heel and the stance hip, respectively,
identied from motion capture. The weighting coecients are listed in Table 6.4 and
were chosen heuristically based on many simulations. The optimization was obtained
in 4 hours with a Pentium II-700 Mhz PC. The resulting gaits, joint positions and joint
torques are shown in Figures 6.96.11 and Appendix D. The good correspondence
with the human data is consistent with previous studies that suggest that human
92
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
Figure 6.9: The resulting gait (duration 0.5 sec) for Case 2: Unimpaired Swing Leg.
The solid lines show the resulting gait and the dashed lines are the gait recorded from
the motion capture system.)
93
0 0.1 0.2 0.3 0.4 0.5
12
10
8
6
4
2
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
4
3
2
1
0
1
2
3
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
10
5
0
5
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
15
10
5
0
5
10
15
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
20
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
80
70
60
50
40
30
20
10
0
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
25
20
15
10
5
0
5
10
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
d
e
g
)
Figure 6.10: The resulting joint motions (duration 0.5 sec) for Case 2: Unimpaired
Swing Leg. The solid lines show the resulting joint motions and the dashed lines are
the joint motions recorded from the motion capture system.)
94
0 0.1 0.2 0.3 0.4 0.5
15
10
5
0
5
10
15
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
300
250
200
150
100
50
0
50
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
20
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
10
5
0
5
10
15
20
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
40
30
20
10
0
10
20
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
15
10
5
0
5
10
15
20
25
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
1.5
1
0.5
0
0.5
1
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
N
m
)
Figure 6.11: The resulting joint torques (duration 0.5 sec) for Case 2: Unimpaired
Swing Leg. The solid lines show the resulting joint torques and the dashed lines are
the joint torques corresponding to the motion capture data.)
95
gait involves the minimization of eort/energy (e.g. [40, 4]). This eort/energy is
applied to lift the swing leg to avoid the ground contact in addition to achieve the
nal conguration. Moreover, the correspondence between the optimized and actual
joint motions (Figure 6.10) suggests that the simulation can adequately predict what
a normative trajectory would be, given only the limb dynamics and desired nal
conguration of the leg.
Because the muscles interacting with the dierent joints have dierent strengths,
dierent weighting coecients, w
ei
s, were chosen for dierent joints. w
e10
was larger
than the rest of w
ei
s which restricts the ankle joint to use less applied torque and/or
eort since the ankle muscle is weaker than the others.
6.3.3 Case 3: Paralyzed Swing Leg with Eort Minimization
of the Stance Hip Torques
To simulate a paralyzed person, the swing hip, knee and ankle joints were
made passive. 16 parameters (8 for each active joint) were used in the optimiza-
tion. Although less parameters are used in this case than in the fully actuated case,
the computation load for integration of the dynamics motion is actually higher. This
is because the dynamics are hybrid with both active and passive joints, which the
previous case had only active joints. The optimization took approximately 3 hours
to complete.
The positions z
1
and z
2
in the penalty function J
p2
were chosen as
z
1
(t) = z
2
(t) = z
stance hip
(t) +
2
3
l
hip
(6.16)
96
w
e4
w
e5
w
p1
w
p2
w
p31
w
p32
10
4
10
4
10
5
10
4
10
3
10
2
Table 6.5: Weighting coecients for swivel in Case 3: Paralyzed Swing Leg
which allows more hip adduction than the previous case. The weighting coecients
are listed in Table 6.5. The results are shown in Figures 6.126.14 and Appendix E.
The optimizer lifted the swing hip to avoid the collision between the legs and between
the swing leg and the ground. At the same time, it twisted the pelvis to pump energy
into the paralyzed leg and moved the leg close to the desired nal conguration. Thus
the optimizer was able to determine a strategy that could achieve repetitive stepping
by shifting the pelvis alone. Note that the strategy incorporated a large swivel by
the stance hip joint around the y-axis which may be undesirable in step training a
real human.
6.3.4 Case 4: Paralyzed Swing Leg with Eort Minimization
of the Stance Hip Torques and Bounded Stance Hip
Orientation
In order to determine if the large swivel motion in the previous case could be
eliminated while still achieving a swing motion, we restricted the stance hip exter-
nal/internal rotation within 30
range by adding the following hard constraint to

the optimization problem
6
q
4

6
, or

6
p
j
4

6
. (6.17)
97
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
Figure 6.12: The resulting gait (duration 0.5 sec) for Case 3: Paralyzed Swing Leg.
98
0 0.1 0.2 0.3 0.4 0.5
60
50
40
30
20
10
0
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
35
30
25
20
15
10
5
0
5
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
20
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
20
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
70
60
50
40
30
20
10
0
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
40
30
20
10
0
10
20
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
d
e
g
)
Figure 6.13: The resulting joint motions (duration 0.5 sec) for Case 3: Paralyzed
99
0 0.1 0.2 0.3 0.4 0.5
800
600
400
200
0
200
400
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
3000
2000
1000
0
1000
2000
3000
4000
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
Figure 6.14: The resulting joint torques (duration 0.5 sec) for Case 3: Paralyzed
The optimization took about 3 hours to complete. The results are shown in Figures
6.156.17 and Appendix F, and demonstrate that a reasonable swing motion can be
achieved while limiting excessive hip swivel.
6.3.5 Comparison of Cases
The applied eort, root-mean-square (RMS) of the position error (compared
to the actual human gait), and norm of the nal position error corresponding to
dierent gait durations for dierent cases are summarized in Figure 6.18, 6.19, and
6.20, respectively. Figure 6.18 demonstrates that most of the eort went into tilting
the pelvis, i.e. abducting the stance hip, due to working against gravity. It also shows
that the underactuated models in Case 3 and 4 need close to the same amount of
eort to shift the stance hip to complete a gait. From Figures 6.19 and 6.20, the fully
actuated model in Case 2 yields the best performance compared to the others. On
one hand, the underactuated model in Case 4 yields a smaller RMS error than that
in Case 3 because of the bound in its stance hip external/internal rotation. On the
100
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
Figure 6.15: The resulting gait (duration 0.5 sec) for Case 4: Paralyzed Swing Leg
with Bounded Stance Hip Orientation. The solid lines show the resulting gait and
the dashed lines are the gait recorded from the motion capture system.)
101
0 0.1 0.2 0.3 0.4 0.5
30
25
20
15
10
5
0
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
35
30
25
20
15
10
5
0
5
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
15
10
5
0
5
10
15
20
25
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
20
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
70
60
50
40
30
20
10
0
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
30
20
10
0
10
20
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
d
e
g
)
Figure 6.16: The resulting joint motions (duration 0.5 sec) for Case 4: Paralyzed
Swing Leg with Bounded Stance Hip Orientation. The solid lines show the resulting
joint motions and the dashed lines are the joint motions recorded from the motion
capture system.)
102
0 0.1 0.2 0.3 0.4 0.5
1000
800
600
400
200
0
200
400
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
3000
2000
1000
0
1000
2000
3000
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
Figure 6.17: The resulting joint torques (duration 0.5 sec) for Case 4: Paralyzed
joint torques and the dashed lines are the joint torques corresponding to the motion
capture data.)
10
0
10
2
10
4
10
6
E
f
f
o
r
t

(
N
2

m
2

s
e
c
)
Figure 6.18: Applied eort for step duration 0.5 sec. (The , , and
marks represent Case 1, 2, 3 and 4, respectively.)
103
0.4 0.45 0.5 0.55 0.6
0
10
20
30
40
50
60
Duration (sec)
R
M
S

(
d
e
g
)
Figure 6.19: Root-Mean-Square of position error. (The , and marks
represent Case 2, 3 and 4, respectively.)
0.4 0.45 0.5 0.55 0.6
0
2
4
6
8
10
Duration (sec)
F
i
n
a
l

P
o
s
i
t
i
o
n

(
d
e
g
)
Figure 6.20: Final position error. (The , and marks represent Case 2, 3
and 4, respectively.)
104
other hand, the bound limits the swivel of the pelvis which causes the underactuated
model in Case 4 to yield a larger nal position error than that in Case 3 for the faster
movements.
6.4 Conclusion
These results demonstrate the possible merits of incorporating control of pelvis
motion for use in robotic body weight supported locomotion training. Although it
may not be possible to fully control swing by manipulating the pelvis, a surprising
amount of control is possible, and the level of control appears sucient for achieving
repetitive stepping for a paralyzed person. Further, the pelvis motions generated to
control swing do not necessarily require large, non-physiological joint movements.
A pelvis-manipulating robot could also be useful for loading the stance leg
by pressing downward on the stance hip, thus providing load-related sensory input
required for stepping at the same time as assisting in swing. An important goal for
future research is to determine how to couple force control of the stance leg with
motion control of the swing leg.
More generally, dynamic motion optimization provides a useful tool for investi-
gating novel strategies for assisting in locomotion rehabilitation. Finding strategies
by observation of therapists is also desirable, but may miss some valuable strategies
because therapists are limited in their control relative to robots. For example, the
strategy found here requires hip torques that are quite large compared to a therapists
strength. Dynamic motion optimization also provides a formal means to automat-
ically generate strategies on a patient-by-patient basis. One can imagine including
105
patient-specic passive joint and reex properties in the simulation. Also, if the pa-
tient begins to recover control over some muscles, it would be possible to model this
activation and include it in the simulation. As the patient recovers walking ability,
one can imagine progressing from unactuated, to partially actuated, to fully actuated
simulations, with the optimization algorithm automatically determining the appro-
priate assistance strategy for each recovery state.
106
Chapter 7
Conclusions
In this dissertation, we used an optimal control formulation to solve a motion
planning problem for both fully-actuated and underactuated robot system. The joint
trajectories are dened by B-spline polynomials. Two applications of dynamic mo-
tion planning are explored: 1) development of point-to-point weightlifting motions
for open-chained robots; and 2) design of robotic gait rehabilitation. The governing
optimal control problem is converted into a direct, SQP parameter optimization in
which the gradient is determined analytically. In the rst application, a weightlifting
motion planner is applied to a Puma 762 robot, with its physical limitations incorpo-
rated into the formulation. The solutions obtained with the motion planner extend
the robots payload capability while reducing the joint torques. In the second appli-
cation, we examine a method to control the stepping motion of a paralyzed person
suspended on a treadmill using a robot attached to the pelvis. A leg swing motion is
created by moving the pelvis without contact with the legs. The resulting motions of
the two applications can be found at the web sites:
1) http://www.eng.uci.edu/chwang/project/puma762.html and
2) http://www.eng.uci.edu/chwang/project/stepper/stepper.html.
107
7.1 Research Summary and Conclusions
In this dissertation, we develop a dynamic motion planner for a robot manipula-
tor to extend its payload capability beyond the limits established by the manufacturer.
The motion planner maximizes the robot payload while taking into account realistic
constraints such as joint torque limits and velocity bounds. It can also minimize joint
torques for a given load, thereby reducing wear on the robot. This work extends
previous results [35], where eort was minimized for systems with end-eector con-
straints. The unique aspects of our approach are: 1) the payload is factored directly
into the cost function for the motion; 2) the semi-innite joint velocity bounds are
transformed to a nite set of linear constraints on the parameters; 3) a time-scale
factor and its gradient are incorporated into the formulation; 4) a linear factorization
of the dynamic system parameters is developed using the geometric operators of Lie
Groups and Lie Algebras; and 5) to the best of our knowledge, this is the rst suc-
cessful application of minimum-eort optimal control to a weight-lifting problem for
a real robot.
Another dynamic motion planner is also developed here to incorporate control of
pelvis motion for use in robotic body-weight supported locomotion training. Although
it may not be possible to fully control swing by manipulating the pelvis, a surprising
amount of control is possible, and the level of control appears sucient for achieving
repetitive stepping for a paralyzed person. Further, the pelvis motions generated
to control swing do not necessarily require large, non-physiological joint movements.
The motion planner can also generate leg swing motions similar to the human gait
during the swing phase.
108
7.2 Future Work
Prospective work is divided into the following interesting areas:
Build a robot for body weight supported training. The resulting motions found
with the dynamic motion planner can be tested on the robot. That would
provide useful information to modify the current motion planner and improve
the design of the robot.
Solve a free nal-time optimal control problem for underactuated robots. Our
current software can only solve the free nal-time optimal control problem for
fully actuated robots. Implementing this function for underactuated robots pro-
vides a means to nd more ecient walking gaits regardless of walking duration
or walking speed.
Generate a complete walking gait. We have generated the stepping motion
during the swing phase of a gait. However, the eort applied to the stance leg
is not taken into account in this work. This has to be done in order to generate
a complete gait.
Create realistic gaits automatically for computer animation characters.
109
Bibliography
[1] L. Lanari A. De Luca and G. Oriolo. A sensitivity approach to optimal spline
robot trajectories. Automatica, 27(3):535539, 1991.
[2] J. V. Albro, G. A. Sohl, J. E. Bobrow, and F. C. Park. On the computation of
optimal high-dives. IEEE International Conference on Robotics and Automation,
4:39583963, April 2000.
[3] C. H. An, C. G. Atkeson, and J. M. Hollerbach. Estimation of inertial parameters
of rigid body links of manipulators. IEEE Conference on Decision and Control,
2:990995, 1985.
[4] F. C. Anderson and M. G. Pandy. Dynamic optimization of human walking.
Journal of Biomechanical Engineering, 123(5):381390, October 2001.
[5] H. Barbeau, K. Norman, J. Fung. M. Visintin, and M. Ladoucer. Does neurore-
habilitation play a role in the recovery of walking in neurological populations.
Annals New York Academy of Sciences, 860:377392, November 1998.
[6] R. H. Bartels, J. C. Beatty, and B. A. Barsky. An Introduction to Splines for
Use in Computer Graphics and Geometric Modeling. Morgan Kaufmann, Las
Altos, California, 1987.
110
[7] J. Baumgarte. Stabilization of constraints and integrals of motion in dynami-
cal systems. Computer Methods in Applied Mechanics and Engineering, 1:116,
1972.
[8] A. Bejczy. Towards development of robotic aid for rehabilitation of locomotion-
impaired subjects. Proceedings of the First Workshop on Robot Motion and
Control (RoMoCo99), pages 916, June 1999.
[9] A. L. Bell, R. A. Brand, and D. R Pedersen. Prediction of hip joint centre
location from external landmarks. Human Movement Science, 8:316, 1989.
[10] J.T. Betts. Survey of numerical methods for trajectory optimization. Journal of
Guidance, Control and Dynamics, 21(2):193207, 1999.
[11] J. E. Bobrow. Optimal robot path planning using the minimum-time criterion.
IEEE Journal of Robotics and Automation, 4(4):443450, 1988.
[12] J. E. Bobrow, C.-Y. E. Wang, and W. K. Timoszyk. Weightlifting motions for a
puma 762 robot. IEEE International Conference on Robotics and Automation,
page V23, May 1999.
[13] R. W. Brockett. Robotic manipulators and the product of exponentials formula.
Mathematical Theory of Networks and Systems, pages 120129, 1984.
[14] A.E. Bryson and Y.C. Ho. Applied Optimal Control. Wiley, New York, New
York, 1995.
[15] B. Cao, G. I. Dodds, and G. W. Irwin. Constrained time-ecient and smooth
cubic spline trajectory generation for industrial robots. IEE Proceedings-Control
Theory and Applications, 144(5):467475, 1997.
111
[16] Y.-C. Chen. Solving robot trajectory planning problems with uniform cubic
B-splines. Optimal Control Applications and Methods, 12(4):247262, 1991.
[17] G. Colombo, M. Joerg, R. Schreier, and V. Dietz. Treadmill training of para-
plegic patients with a robotic orthosis. Journal of Rehabilitation Research and
Development, 37(6), November 2000.
[18] J. J. Craig. Introduction to Robotics. Addison-Wesley, Reading, Massachusetts,
1989. Second Edition.
[19] Carlos Canudas de Wit, Bruno Siciliano, and Georges Bastin. Theory of Robot
Control. Springer-Verlag, New York, New York, 1996.
[20] W. T. Dempster and G. R. L. Gaughran. Properties of body segments based on
size and weight. American Journal of Anatomy, 120:3354, 1968.
[21] J. Denavit and R. S. Hartenberg. A kinematic notation for lower-pair mech-
anisms based on matrices. ASME Journal of Applied Mechanics, 22:215221,
1955.
[22] Eric R. Dunn and Robert D. Howe. Towards smooth bipedal walking. IEEE
International Conference on Robotics and Automation, 3:24892494, May 1994.
[23] Roy Featherstone. Robot Dynamics Algorithms. Kluwer Academic Publishers,
Norwell, Massachusetts, 1987.
[24] G. Field and Y. Stepanenko. Iterative dynamic programming: an approach
to minimum energy trajectory planning for robotic manipulators. IEEE
International Conference on Robotics and Automation, 3:27552760, April 1996.
112
[25] Michael Hardt, Kenneth Kreutz-Delgado, and J. William Helton. Minimal en-
ergy control of a biped robot with numerical methods and a recursive sym-
bolic dynamic model. IEEE Conference on Decision and Control, 1:413416,
December 1998.
[26] S. Hesse and D. Uhlenbrock. A mechanized gait trainer for restoration of gait.
Journal of Rehabilitation Research and Development, 37(6), November 2000.
[27] S. Hesse, D. Uhlenbrock, and T. Sarkodie-Gyan. Gait pattern of severely disabled
hemiparetic subjects on a new controlled gait trainer as compared to assisted
treadmill walking with partial body weight support. Clinical Rehabilitation,
13(5):401410, October 1999.
[28] Jessica K. Hodgins. Three-dimensional human running. IEEE International
Conference on Robotics and Automation, 4:32713276, April 1996.
[29] M. Zefran J.P. Desai and V Kumar. Two-arm manipulation tasks with friction-
assisted grasping. Advances in Robotics, 12(5):485507, 1999.
[30] R. N. Kirkwood, E. G. Culham, and P. Costigan. Radiographic and non-invasive
determination of the hip joint center location: eect on hip joint moments.
Clinical Biomechanics, 14(4):227235, May 1999.
[31] Richard H. Lathrop. Constrained (closed-loop) robot simulation by local
constraint propagation. IEEE International Conference on Robotics and
Automation, 2:689694, April 1986.
[32] Jean-Claude Latombe. Robot Motion Planning. Kluwer Academic Publishers,
Boston, Massachusetts, 1991.
113
[33] S. Liu and J. E. Bobrow. An analysis of a pneumatic servo systme and its
application to a computer-controlled robot. ASME Journal of Dynamic Systems,
Measurement, and Control, 110:228235, September 1988.
[34] D. G. Luenberger. Linear and Nonlinear Programming. Addison-Wesley,
Reading, Massachusetts, 1989. Second Edition.
[35] B. J. Martin and J. E. Bobrow. Minimum eort motions for open chain manip-
ulators with task-dependent end-eector constraints. International Journal of
Robotics Research, 18(2):213224, 1999.
[36] Bryan J. Martin. Robot Motion Optimization and Path Planning, Ph.D. Thesis.
University of California, Irvine, California, 1996.
[37] J. M. McCarthy. An Introduction to Theoretical Kinematics. The MIT Press,
Cambridge, Massachusetts, 1990.
[38] R. M. Murray, Z. Li, and S. S. Sastry. A Mathematical Introduction to Robot
Manipulation. CRC Press, Boca Raton, Florida, 1993.
[39] H. Ozaki and C.-J. Lin. Optimal B-spline joint trajectory generation for collision-
free movements of a manipulator under dynamic constraints. IEEE International
Conference on Robotics and Automation, 4:35923597, April 1996.
[40] M. G. Pandy and F. C. Anderson. Dynamic simulation of human movement using
large-scale models of the body. IEEE International Conference on Robotics and
[41] F. C. Park and J. E. Bobrow. A recursive algorithm for robot dynamics using
Lie groups. IEEE International Conference on Robotics and Automation, 2:1535
1540, May 1994.
114
[42] F. C. Park, J. E. Bobrow, and S. R. Ploen. A Lie group formulation of robot
dynamics. International Journal of Robotics Research, 14(6):609618, 1995.
[43] Frank C. Park. The Optimal Kinematic Design of Mechanisms, Ph.D. Thesis.
Harvard University, Cambridge, Massachusetts, 1991.
[44] J.-K. Park and J. E. Bobrow. Minimum-time trajectory planning for a robot
manipulator amid obstacles. Proceedings of the 4
th
Japan-France Congress and
2
nd
Asia-Europe Congress on Mechatronics, 1:369374, 1998.
[45] Scott R. Ploen. Geometric Algorithms for the Dynamics and Control of
Multibody Systems, Ph.D. Thesis. University of California, Irvine, California,
1997.
[46] J. Pratt, P. Dilworth, and G. Pratt. Virtual model control of a bipedal walking
robot. IEEE International Conference on Robotics and Automation, 1:193198,
April 1997.
[47] J. Pratt and G. Pratt. Intuitive control of a planar bipedal walking robot. IEEE
International Conference on Robotics and Automation, 3:20142021, May 1998.
[48] J. Pratt, A. Torres, P. Dilworth, and G. Pratt. Virtual actuator control.
IEEE International Conference on Intelligent Robots and Systems, 3:12191226,
November 1996.
[49] Marc H. Raibert. Legged Robots Tahe Balance. MIT Press, Cambridge,
Massachusetts, 1986.
115
[50] D.J. Reinkensmeyer, W.K. Timoszyk, R.D. de Leon RD, R. Joynes, E. Kwak,
K. Minakata, and V.R. Edgerton. A robotic stepper for retraining locomo-
tion in spinal-injured rodents. IEEE International Conference on Robotics and
[51] G Rodriguez, A. Jain, and K. Kreutz-Delgado. A spatial operator algebra for
manipulator modeling and control. International Journal of Robotics Research,
10(4):371381, August 1991.
[52] G Rodriguez, A. Jain, and K. Kreutz-Delgado. Spatial operator algebra for
multibody system dynamics. Journal of Astronautical Sciences, 40(1):2750,
1992.
[53] G. A. Sohl and J. E. Bobrow. Optimal motions for underactuated manipulators.
ASME Design Engineering Technical Conferences, September 1999.
[54] S. E. Thompson and R. V. Patel. Formulation of joint trajectories for industrial
robots using B-splines. IEEE Transaction on Industrial, Electronics, 34(2):192
199, 1987.
[55] Michiel van de Panne. Parameterized gait synthesis. IEEE Computer Graphics
and Applications, 16(2):4049, March 1996.
[56] Michiel van de Panne. From footprints to animation. Computer Graphics Forum,
16(4):211223, October 1997.
[57] C.-Y. E. Wang, J. E. Bobrow, and D. J. Reinkensmeyer. Swinging from the hip:
use of dynamic motion optimization in the design of robotic gait rehabilitation.
IEEE International Conference on Robotics and Automation, 2:14331438, May
2001.
116
[58] C.-Y. E. Wang, W. K. Timoszyk, and J. E. Bobrow. Weightlifting motion plan-
ning for a puma 762 robot. IEEE International Conference on Robotics and
Automation, 1:480485, May 1999.
[59] C.-Y. E. Wang, W. K. Timoszyk, and J. E. Bobrow. Payload maximization for
open chained manipulators: nding weightlifting motions for a puma 762 robot.
IEEE Transactions on Robotics and Automation, 17(2):218224, 2001.
[60] Jerey M. Wendlandt. A recursive workspace balancing controller for a 3D
multibody model of a biped. IEEE Conference on Decision and Control, 5:4838
4843, December 1997.
[61] Jerey M. Wendlandt and S. Shankar Sastry. Recursive workspace control of
multibody systems: a planar biped example. IEEE Conference on Decision and
Control, 3:35753580, December 1996.
[62] I. Wickelgren. Teaching the spinal cord to walk. Science, 279:319321, January
1998.
[63] V. Zatsiorsky and V. Seluyanov. Estimation of the mass and inertia character-
istics of the human body by means of the best prediictive regression equations.
Biomechanics IX-B (Edited by D. A. Winter et al., Human Kinetics Publishers,
Champain, Illinois), pages 233239, 1985.
117
Appendix A
Transformation Between Joints
and Motion Capture Markers
In the motion capture, external markers were attached to the human subject at
the antero-superior iliac spines (ASISs), knees, ankles, tops of the feet, and backs of
the heels. Cartesian coordinates of the markers were recorded and converted to joint
angles and link lengths using a least squares method and forward kinematics.
The distance, l
Asis
, between the subjects left and right ASISs was 0.275 m. The
hip joint center location was estimated from the right and left ASISs based on Bells
method [9]: the right hip joint is located at 14% of l
Asis
medial to the right ASIS,
30% of l
Asis
distal to the right ASIS and 22% of l
Asis
posterior to the right ASIS. An
additional adjustment was made by the dierence in the results from Bells method
and precise measurement by the QUESTOR Precision Radiography (QPR) system
[30]: 0.026 m is added medial to the right ASIS; 0.01 m distal to the right ASIS; and
0.005 m anterior to the right ASIS.
The widths of the subjects knee and ankle joints are listed in Table A.1. Markers
attached at the ASISs and knees were 0.03 m in diameter and those attached at the
ankles, tops of the toes and backs of the heels were 0.02 m in diameter. Because
the ASIS markers were actually attached to the subjects clothes which resulted in
inaccurate position measurement, a bias of 0.01 m was added to the position of left
118
Right Knee Left Knee Right Ankle Left Ankle
0.094 m 0.087 m 0.074 m 0.07 m
Table A.1: Joint width
ASIS marker in the ydirection and was subtracted from that of right ASIS marker
in the same direction.
We chose the data of one-half of the gait cycle in which left and right legs are
stance and swing legs, respectively. The following forward kinematics represents the
homogeneous coordinate transformation from the joint angles and link lengths to the
positions of markers.
x
LHip
= (q
1
, q
2
, q
3
, 1) (A.1)
x
LAsis
= x
LHip
+T
LHip
(d
1
+ 0.03/2, d
2
+ 0.01, d
3
, 1) (A.2)
x
RAsis
= x
LHip
+T
LHip
(d
1
+ 0.03/2, d
2
0.01, d
3
+l
Hip
, 1) (A.3)
x
RKnee
= x
LHip
+T
RHip
(0, l
ULeg
, 0.094/2 + 0.03/2, 1) (A.4)
x
RAnkle
= x
LHip
+T
RKnee
(0, l
LLeg
, 0.074/2 + 0.02/2, 1) (A.5)
x
RHeel
= x
LHip
+T
RAnkle
(l
Heel
0.02/2, l
Foot
+ 0.01, 0, 1) (A.6)
x
RToe
= x
LHip
+T
RAnkle
(l
Toe
, l
Foot
+ 0.03 + 0.02/2, 0, 1) (A.7)
where
T
LHip
= exp(q
4
S
ry
)exp(q
5
S
rx
) (A.8)
T
RHip
= T
LHip
exp(l
Hip
S
tz
)exp(q
6
S
rz
)exp(q
7
S
rx
)exp(q
8
S
ry
) (A.9)
T
RKnee
= T
RHip
exp(l
ULeg
S
ty
)exp(q
9
S
rz
) (A.10)
T
RAnkle
= T
RKnee
exp(l
LLeg
S
ty
)exp(q
10
S
rz
) (A.11)
d
1
= .22l
Asis
0.005 (A.12)
119
d
2
= .3l
Asis
+ 0.01 (A.13)
d
3
= .14l
Asis
+ 0.026. (A.14)
The homogeneous coordinate is x where the subscript represents the corresponding
joint or marker and its rst letter represents Right or Left. The homogeneous
coordinate transformation is T. S
rx
, S
ry
and S
rz
represent the screw axes of rotation
about the x-, y- and z-axes, respectively; and S
tx
, S
ty
and S
tz
the screw axes of
translation along the x-, y- and z-axes, respectively. The distance between right
and left hips is l
Hip
; and the lengths of upper and lower legs are l
ULeg
and l
LLeg
,
respectively. Assuming the subject stands straight up, l
Foot
represents the vertical
distance between the ankle and sole of the foot (ground); l
Toe
the horizontal distance
between the ankle and toes; and l
Heel
the horizontal distance between the ankle and
heel.
The forward kinematics shows that the position of marker x is a function of q
and l, or x = x(q, l), in which x is known and q and l are unknown. A least squares
method was used to solve for q and l:
min
q,l
122
j=1
[[x
marker j
x
j
(q, l)[[
2
(A.15)
for each sampling instance. The resulting link lengths are shown in Table 6.1.
120
Appendix B
Estimation of Dynamical
Properties Using Regression
Equations
The dynamical properties of the human subjects body segments were estimated
using regression equations introduced by Zatsiorsky & Seluyanov [63]. The following
measurements were taken on the human subject in order to compute the dynamical
properties: length of the foot (27.9 cm), width of the foot (10.2 cm), fat (75 kg),
length of the lower leg (48.9 cm), lower diameter of the lower leg (7.6 cm), proxi-
mal circumference of the lower leg (33.0 cm), distal circumference of the lower leg
(20.3 cm), maximal circumference of the lower leg (36.8 cm), length of the upper leg
(47.0 cm), lower diameter of the upper leg (12.7 cm), distal circumference of the up-
per leg (38.1 cm), middle circumference of the upper leg (45.7 cm), and circumference
of the upper leg at the infragluteal fold (57.2 cm).
The location of the center of mass of the upper leg was determined to be 12.4 cm
distal to the ASIS using the regression equations. From a visual inspection, 12.4 cm
did not appear correct (the length of the upper leg of 47.0 cm), and was replaced by
17.0 cm based on a volumetric approach [28], using density estimated from [20]. The
principal inertia and center of mass of the upper trunk were determined based on the
same approach. The results are listed in Table 6.2.
121
Appendix C
Results for Gait Rehabilitation
Case 1: Paralyzed Swing Leg with
Motion Captured Stance Hip
Orientation
122
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
Figure C.1: The resulting gait (duration 0.433 sec) for Case 1: Paralyzed Swing Leg
capture system.)
123
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
capture system.)
124
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
capture system.)
125
Appendix D
Case 2: Unimpaired Swing Leg
with Eort Minimization of All
Joints
126
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
Figure D.1: The resulting gait (duration 0.433 sec) for Case 2: Unimpaired Swing
Leg. The solid lines show the resulting gait and the dashed lines are the gait recorded
from the motion capture system.)
127
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
128
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
129
0 0.1 0.2 0.3 0.4 0.5
8
6
4
2
0
2
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
1
0
1
2
3
4
5
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
10
5
0
5
10
15
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
25
20
15
10
5
0
5
10
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
20
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
80
70
60
50
40
30
20
10
0
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
30
20
10
0
10
20
30
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
d
e
g
)
Figure D.4: The resulting joint motions (duration 0.433 sec) for Case 2: Unimpaired
130
0 0.1 0.2 0.3 0.4 0.5
20
15
10
5
0
5
10
15
20
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
200
150
100
50
0
50
100
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
20
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
20
15
10
5
0
5
10
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
80
60
40
20
0
20
40
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
30
20
10
0
10
20
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
2
1.5
1
0.5
0
0.5
1
1.5
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
N
m
)
Figure D.5: The resulting joint torques (duration 0.433 sec) for Case 2: Unimpaired
131
0 0.1 0.2 0.3 0.4 0.5
10
9
8
7
6
5
4
3
2
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
3
2
1
0
1
2
3
4
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
10
5
0
5
10
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
10
5
0
5
10
15
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
80
70
60
50
40
30
20
10
0
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
25
20
15
10
5
0
5
10
15
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
d
e
g
)
132
0 0.1 0.2 0.3 0.4 0.5
100
50
0
50
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
250
200
150
100
50
0
50
100
150
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
15
10
5
0
5
10
15
20
25
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
6
4
2
0
2
4
6
8
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
40
30
20
10
0
10
20
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
20
15
10
5
0
5
10
15
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
1.5
1
0.5
0
0.5
1
1.5
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
N
m
)
133
0 0.1 0.2 0.3 0.4 0.5 0.6
15
10
5
0
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
6
5
4
3
2
1
0
1
2
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
10
5
0
5
10
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
15
10
5
0
5
10
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
10
5
0
5
10
15
20
25
30
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
80
70
60
50
40
30
20
10
0
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
20
15
10
5
0
5
10
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
d
e
g
)
134
0 0.1 0.2 0.3 0.4 0.5 0.6
20
15
10
5
0
5
10
15
20
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5 0.6
200
150
100
50
0
50
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5 0.6
20
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5 0.6
4
2
0
2
4
6
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5 0.6
30
20
10
0
10
20
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5 0.6
10
5
0
5
10
15
20
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5 0.6
0.5
0
0.5
1
1.5
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
N
m
)
135
Appendix E
Eort Minimization of the Stance
Hip Torques
136
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
Figure E.1: The resulting gait (duration 0.433 sec) for Case 3: Paralyzed Swing Leg.
137
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
138
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
139
0 0.1 0.2 0.3 0.4 0.5
60
50
40
30
20
10
0
10
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
35
30
25
20
15
10
5
0
5
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
10
5
0
5
10
15
20
25
30
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
20
10
0
10
20
30
40
50
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
20
10
0
10
20
30
40
50
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
80
70
60
50
40
30
20
10
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
40
30
20
10
0
10
20
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
d
e
g
)
Figure E.4: The resulting joint motions (duration 0.433 sec) for Case 3: Paralyzed
140
0 0.1 0.2 0.3 0.4 0.5
1000
800
600
400
200
0
200
400
600
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
4000
3000
2000
1000
0
1000
2000
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
Figure E.5: The resulting joint torques (duration 0.433 sec) for Case 3: Paralyzed
141
0 0.1 0.2 0.3 0.4 0.5
40
30
20
10
0
10
20
30
40
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
40
30
20
10
0
10
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
20
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
50
40
30
20
10
0
10
20
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
80
70
60
50
40
30
20
10
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
30
20
10
0
10
20
30
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
d
e
g
)
142
0 0.1 0.2 0.3 0.4 0.5
2000
1500
1000
500
0
500
1000
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
2000
1000
0
1000
2000
3000
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
143
0 0.1 0.2 0.3 0.4 0.5 0.6
20
10
0
10
20
30
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
60
50
40
30
20
10
0
10
20
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
80
60
40
20
0
20
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
10
5
0
5
10
15
20
25
30
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
80
70
60
50
40
30
20
10
0
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
30
20
10
0
10
20
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
d
e
g
)
144
0 0.1 0.2 0.3 0.4 0.5 0.6
400
300
200
100
0
100
200
300
400
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5 0.6
2000
1500
1000
500
0
500
1000
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
145
Appendix F
Eort Minimization of the Stance
Hip Torques and Bounded Stance
Hip Orientation
146
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
Figure F.1: The resulting gait (duration 0.433 sec) for Case 4: Paralyzed Swing Leg
147
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
148
100 50 0 50 100
10
0
10
20
x (cm)
z

(
c
m
)
100 50 0 50 100
0
20
40
60
80
100
120
x (cm)
y

(
c
m
)
10 0 10 20
0
20
40
60
80
100
120
z (cm)
y

(
c
m
)
149
0 0.1 0.2 0.3 0.4 0.5
30
25
20
15
10
5
0
5
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
40
30
20
10
0
10
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
15
10
5
0
5
10
15
20
25
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
20
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
70
60
50
40
30
20
10
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
30
20
10
0
10
20
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
d
e
g
)
Figure F.4: The resulting joint motions (duration 0.433 sec) for Case 4: Paralyzed
capture system.)
150
0 0.1 0.2 0.3 0.4 0.5
1000
800
600
400
200
0
200
400
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
3000
2000
1000
0
1000
2000
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
Figure F.5: The resulting joint torques (duration 0.433 sec) for Case 4: Paralyzed
capture data.)
151
0 0.1 0.2 0.3 0.4 0.5
30
20
10
0
10
20
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
40
30
20
10
0
10
20
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
20
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
30
25
20
15
10
5
0
5
10
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
80
70
60
50
40
30
20
10
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5
30
20
10
0
10
20
30
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
d
e
g
)
capture system.)
152
0 0.1 0.2 0.3 0.4 0.5
1200
1000
800
600
400
200
0
200
400
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5
4000
3000
2000
1000
0
1000
2000
3000
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
capture data.)
153
0 0.1 0.2 0.3 0.4 0.5 0.6
20
10
0
10
20
30
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
60
50
40
30
20
10
0
10
20
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
10
0
10
20
30
40
t (sec)
S
w
i
n
g

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
80
60
40
20
0
20
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
10
5
0
5
10
15
20
25
30
t (sec)
S
w
i
n
g

h
i
p

e
x
t
e
n
s
i
o
n
/
f
l
e
x
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
80
70
60
50
40
30
20
10
0
t (sec)
K
n
e
e

f
l
e
x
i
o
n
/
e
x
t
e
n
s
i
o
n

(
d
e
g
)
0 0.1 0.2 0.3 0.4 0.5 0.6
30
20
10
0
10
20
t (sec)
A
n
k
l
e

p
l
a
n
t
a
r
/
d
o
r
s
a
l

f
l
e
x
i
o
n

(
d
e
g
)
capture system.)
154
0 0.1 0.2 0.3 0.4 0.5 0.6
400
300
200
100
0
100
200
300
400
t (sec)
S
t
a
n
c
e

h
i
p

e
x
t
e
r
n
a
l
/
i
n
t
e
r
n
a
l

r
o
t
a
t
i
o
n

(
N
m
)
0 0.1 0.2 0.3 0.4 0.5 0.6
2000
1500
1000
500
0
500
1000
t (sec)
S
t
a
n
c
e

h
i
p

a
b
d
u
c
t
i
o
n
/
a
d
d
u
c
t
i
o
n

(
N
m
)
capture data.)
155

Belajar Dynamic 2

Enviado por

Dados do documento

Direitos autorais

Formatos disponíveis

Compartilhar este documento

Compartilhar ou incorporar documento

Opções de compartilhamento

Você considera este documento útil?

Este conteúdo é inapropriado?

Direitos autorais:

Formatos disponíveis

Belajar Dynamic 2

Enviado por

Direitos autorais:

Formatos disponíveis

UNIVERSITY OF CALIFORNIA

is the position of a rigid point represented in one coordinate frame and

and can be written as

operators can be easily obtained using the chain

range by adding the following hard constraint to

Você também pode gostar