In this paper, we present a method to initialize at a feasible point and unfailingly solve a non-convex optimization problem in which a set-point motion is planned for a multi-link manipulator under state and control constraints. We construct an initial feasible solution by analyzing the final time effect for feasibility problems of flatness based motion planning problems. More specifically, we first find a feasible time-optimal trajectory under state constraints without a control constraint by solving a linear programming problem. Then, we find a feasible trajectory under control constraints by scaling the trajectory. To evaluate the practical applicability of the proposed method, we did numerical experiments to solve a multi-link manipulator motion planning problem by combining the method with recursive inverse dynamics algorithms.
 In this article we present a unified geometric treatment of robot dynamics. Using standard ideas from Lie groups and Rieman nian geometry, we formulate the equations of motion for an open chain manipulator both recursively and in closed form. The recursive formulation leads to an O(n) algorithm that ex presses the dynamics entirely in terms of coordinate-free Lie algebraic operations. The Lagrangian formulation also ex presses the dynamics in terms of these Lie algebraic operations and leads to a particularly simple set of closed-form equations, in which the kinematic and inertial parameters appear explic itly and independently of each other. The geometric approach permits a high-level, coordinate-free view of robot dynamics that shows explicitly some of the connections with the larger body of work in mathematics and physics. At the same time the resulting equations are shown to be computationally ef fective and easily differentiated and factored with respect to any of the robot parameters. This latter fe... INTRODUCTION: Brief History. Multifingered Hands and Dextrous Manipulation. Outline of the Book. Bibliography. RIGID BODY MOTION: Rigid Body Transformations. Rotational Motion in R3. Rigid Motion in R3. Velocity of a Rigid Body. Wrenches and Reciprocal Screws. MANIPULATOR KINEMATICS: Introduction. Forward Kinematics. Inverse Kinematics. The Manipulator Jacobian. Redundant and Parallel Manipulators. ROBOT DYNAMICS AND CONTROL: Introduction. Lagrange's Equations. Dynamics of Open-Chain Manipulators. Lyapunov Stability Theory. Position Control and Trajectory Tracking. Control of Constrained Manipulators. MULTIFINGERED HAND KINEMATICS: Introduction to Grasping. Grasp Statics. Force-Closure. Grasp Planning. Grasp Constraints. Rolling Contact Kinematics. HAND DYNAMICS AND CONTROL: Lagrange's Equations with Constraints. Robot Hand Dynamics. Redundant and Nonmanipulable Robot Systems. Kinematics and Statics of Tendon Actuation. Control of Robot Hands. NONHOLONOMIC BEHAVIOR IN ROBOTIC SYSTEMS: Introduction. Controllability and Frobenius' Theorem. Examples of Nonholonomic Systems. Structure of Nonholonomic Systems. NONHOLONOMIC MOTION PLANNING: Introduction. Steering Model Control Systems Using Sinusoids. General Methods for Steering. Dynamic Finger Repositioning. FUTURE PROSPECTS: Robots in Hazardous Environments. Medical Applications for Multifingered Hands. Robots on a Small Scale: Microrobotics. APPENDICES: Lie Groups and Robot Kinematics. A Mathematica Package for Screw Calculus. Bibliography. Index Each chapter also includes a Summary, Bibliography, and Exercises We propose a fast physically-based simulation system for skeleton-driven deformable body characters. Our system can generate realistic motions of self-propelled deformable body characters by considering the two-way interactions among the skeleton, the deformable body, and the environment in the dynamic simulation. It can also compute the passive jiggling behavior of a deformable body driven by a kinematic skelet al motion. We show that a well-coordinated combination of: (1) a reduced deformable body model with nonlinear finite elements, (2) a linear-time algorithm for skeleton dynamics, and (3) explicit integration can boost simulation speed to orders of magnitude faster than existing methods, while preserving modeling accuracy as much as possible. Parallel computation on the GPU has also been implemented to obtain an additional speedup for complicated characters. Detailed discussions of our engineering decisions for speed and accuracy of the simulation system are presented in the article. We tested our approach with a variety of skeleton-driven deformable body characters, and the tested characters were simulated in real time or near real time. In this work an efficient dynamics algorithm is developed, which is applicable to a wide range of multibody systems, including underactuated systems, branched or tree-topology systems, robots, and walking machines. The dynamics algorithm is differentiated with respect to the input parameters in order to form sensitivity equations. The algorithm makes use of techniques and notation from the theory of Lie groups and Lie algebras, which is reviewed briefly. One of the strengths of our formulation is the ability to easily differentiate the dynamics algorithm with respect to parameters of interest. We demonstrate one important use of our dynamics and sensitivity algorithms by using them to solve difficult optimal control problems for underactuated systems. The algorithms in this paper have been implemented in a software package named Cstorm (Computer simulation tool for the optimization of robot manipulators), which runs from within Matlab and Simulink. It can be downloaded from the website http: www.eng.uci.edu bobrow @DOI: 10.1115 1.1376121#