%TEMPLATE FOR PRODUCING IEEE-FORMAT ARTICLE FOR ICRA2000 USING LaTeX. %written by Matthew Ward, CS Department, Worcester Polytechnic Institute. %use at your own risk. Complaints to /dev/null. %make two column with no page numbering, default is 10 point \documentclass[twocolumn]{article} \pagestyle{empty} %set dimensions of columns, gap between columns, and space between paragraphs \setlength{\textheight}{8.75in} \setlength{\columnsep}{2.0pc} \setlength{\textwidth}{6.8in} %\setlength{\footheight}{0.0in} \setlength{\topmargin}{0.25in} \setlength{\headheight}{0.0in} \setlength{\headsep}{0.0in} \setlength{\oddsidemargin}{-.19in} \setlength{\parindent}{0pc} \setlength{\parskip}{0.03in} %I copied stuff out of art10.sty and modified them to conform to IEEE format \makeatletter %as LaTeX considers descenders in its calculation of interline spacing, %to get 12 point spacing for normalsize text, must set it to 10 points \def\@normalsize{\@setsize\normalsize{12pt}\xpt\@xpt \abovedisplayskip 10pt plus2pt minus5pt\belowdisplayskip \abovedisplayskip \abovedisplayshortskip \z@ plus3pt\belowdisplayshortskip 6pt plus3pt minus3pt\let\@listi\@listI} %need an 11 pt font size for subsection and abstract headings \def\subsize{\@setsize\subsize{12pt}\xipt\@xipt} %% OLD SECTION COMMANDS % %%make section titles bold and 12 point, 2 blank lines before, 1 after %\def\section{\@startsection {section}{1}{\z@}{24pt plus 2pt minus 2pt} %{12pt plus 2pt minus 2pt}{\large\bf}} %%%make subsection titles bold and 11 point, 1 blank line before, 1 after %\def\subsection{\@startsection {subsection}{2}{\z@}{12pt plus 2pt minus 2pt} %{12pt plus 2pt minus 2pt}{\subsize\bf}} \makeatother \begin{document} %don't want date printed \date{} %make title bold and 14 pt font (LaTeX default is non-bold, 16 pt) \title{\Large\bf My Wonderful Article in IEEE Format for ICRA2004} %for single author (just remove % characters) \author{J. V. Albro, J.E. Bobrow \\ Department of Mechanical and Aerospace Engineering\\ University of California, Irvine\\ Irvine, CA~~92697} %for two authors (this is what is printed) %\author{\begin{tabular}[t]{c@{\extracolsep{8em}}c} % J. V. Albro & J. E. Bobrow \\ % \\ % Department of Mechanical and Aerospace Engineering\\ % University of California, Irvine\\ % Irvine, CA~~92697 %\end{tabular}} \maketitle %I don't know why I have to reset thispagesyle, but otherwise get page numbers \thispagestyle{empty} \subsection*{\centering Abstract} %IEEE allows italicized abstract {\em This is the abstract of my paper. It must fit within the size allowed, which is about 3 inches, including section title, which is 11 point bold font. If you don't want the text in italics, simply remove the 'em' command and the curly braces which bound the abstract text. If you have em commands within an italicized abstract, the text will come out as normal (nonitalicized) text. %end italics mode } \section{Introduction} Our purpose in this research was to find motion segments for robots that make and break contact with the ground, such as hopping, walking, or tumbling robots. %Our previous work had dealt with fully actuated robots %fixed to the ground at some point, such as the PUMA 762, or free-flying %systems that could be modeled with underactuated robots, such as the diver. In our previous work, we had successfully found some motions for a Luxo robot that made contact with the ground, and for a model of human walking, but in both cases the contact model was formulated specifically for those systems. Our goal here was to find a general formulation for contact that would be calculated automatically as part of our existing C-STORM software for modeling the dynamics of robot systems. This would allow us to do path planning for a much more general class of robots and physical systems. We use an optimal control approach to generating trajectories for robots. %(Why the heck do we use the optimal control approach? Help!) %(look at papers for more reasons). Using optimal control to generate paths allows the paths to be ``good" according to some objective measure, does not require a large amount of a priori information about the motion, and allows the motion to be specified in a more high-level manner (like ``Go that way as far as you can while using energy efficiently."). For example, by using different objective measures, one can find trajectories that maximize fuel efficiency, minimize the time to travel a given trajectory, to extend the capabilities of an existing robot (cite PUMA paper), or in the design phase to select the physical parameters of a robot being designed to better fit its intended function. Optimal control has been used many times to generate trajectories for physical systems (including robots), (cite a selection of people) but the complexity of the governing dynamics equations often renders the problem intractable (cite Betts). Many of the algorithms used to solve these types of problems perform poorly when finite difference gradients of the objective function and dynamics equations are used. For ill-conditioned systems, such as (those that make and break contact with the ground, or have nonholonomic constraints) approximate gradients are especially problematic. The implication for the physical model of the robot is that the functions used in it need to have continuous derivatives whose analytic form can be determined. Previously, by using the matrix exponential formulation of the dynamics and Lie group theory, we could express the equations of motion (and thus, the objective function) so that their analytic gradients came automatically from the formulation of the equations. This was implemented in C-STORM (cite refs), software we wrote to solve these types of problems. In this research, we wish to extend C-STORM's applicability to robots that make intermittent contact with the environment in a general way. C-STORM had been used to solve hopping robot (cite ref) and human walking (cite ref) problems, but in both these cases, modeling the interaction with the environment was specific to the particular system being simulated. The difficulty in modeling systems that make and break contact with the ground is that the dynamics of the system change in different phases of the motion. Even in motion as prosaic as walking, the dynamics of the system when standing on one leg differs from when standing on two legs. There is the added difficulty of doing it in a general way, so that once the system and its environment are modeled, contact is dealt with as part of an algorithm rather than having to be implemented for that specific system. There are two general approaches for dealing with the difficulty of changing dynamics: having different dynamic models for the different phases of motion, and switching between them as needed; or having one dynamics model throughout and modeling interactions with the environment as external forces acting on the system model. There are advantages and disadvantages to both approaches \cite{Betts98,Buss02}. One of the advantages of the switching method is that it reduces the degrees of freedom in different phases of the motion, simplifying the dynamics model; also, interaction forces from the environment are added as constraints on the system, the same way the dynamics equations are modeled, giving a straightforward way to deal with constraints. A large disadvantage is that there has to be some way to determine which dynamics model ought to be used, how and when to switch between them, and the state space gets larger with every additional dynamics model that is needed. (cite the German paper) We have chosen to use one dynamics model throughout the simulation, and to model the contact forces with the environment as external forces on the system. This method poses its own set of challenges but does eliminate the problem of switching between dynamic models and trying to determine which model the simulation should be using at any particular time. The particular down side of this approach, versus the switching approach, is that contact with the environment needs to be modeled, rather than just included in the constraints on the system (as is done in the switching approach). Because of the way we find motions for our systems, we need the model for the environment to be continuous and have a continuous first derivative, which adds to the difficulty of modeling contact forces. Yamane and Nakamura \cite{YamaneNakamura03} give a good overview of how contact models are implemented. There are two categories of contact models, penalty-based methods and analytical methods. In the penalty-based methods, which we use, contact forces are modeled with virtual spring-damper systems at points of contact. %\begin{figure} %\centerline{\psfig{file=springdamper.eps,height=2.0in}} %% %\caption{Virtual Spring-Damper System Modeling Contact Forces} %\label{fig:springdamper} %\end{figure} This is straightforward to implement, but it means integrating a stiff system, so simulation is slowed down and requires more precision than finite difference gradients can provide. Additionally finding good parameters for the spring and damper constants is difficult and must often be tuned for the particular system being modeled. Wang, Kumar, and Abel \cite{WangKumarAbel92} looked more closely at the rigid body assumption in contact and how it can lead to violations of conservation of energy and other conservation laws. They simulate mechanical systems undergoing multiple frictional constraints with local, linearly elastic properties at points of contact. Our contact model has similarities to this approach, as well as that of Delcomyn et at (cite ref), and is a modification of the approach described in Tenaglia and Orin (cite ref). Having to find the derivative of the contact forces, which are functions of distances between colliding bodies and the relative velocity of those bodies, requires also finding the derivative of how the distance between the two bodies changes. More specifically, to model contact forces on the robot, we have to be able to determine the distances between surfaces of the robot and other objects, the location of the near points on the robot and environmental objects, and how these near points move as the robot moves. We have developed an approach that allows us to describe the necessary distance derivatives so that the collision forces are continuous and defined. This paper will describe how we find motion segments for robots that make and break contact with the ground. First, the overall optimal control approach will be described. Because of the choice to use an optimal control approach in conjunction with using one model throughout the motion, the necessary contact model for external forces on the robot will be described. The procedure for finding the distance derivatives needed for the contact model and to have the optimizations converge for the optimal control approach will be detailed, and finally, the simulation results and conclusions will be discussed. \section{Generating Motions} Our approach to finding motions for our systems of interest is to solve for the control input which results in the desired motion. %%(The system of interest will subsequently be referred %%to as a robot for convenience, even when the system is biological.) To find these controls, we formulate the appropriate optimal control problem and solve it to obtain the motion of the robot. We transform the optimal control problem into a static parameter optimization which generates controls that satisfy the cost function, the dynamics equations, boundary conditions, and physical limits on the system. %while still reaching some desired final %position. Our software program C-STORM allows a user to specify the physical characteristics of the robot and generates its associated equations of motion automatically. Used in conjunction with MATLAB's built-in functions to perform parameter optimization, this allows the solution for the desired control inputs. To get controls for our robots, we assume the controls minimize some cost function. Each cost function corresponds to a unique motion, so specifying the cost function equates to specifying the motion. The cost function is minimized subject to the equations of motion of the system, boundary conditions on certain joint positions and velocities, and joint limits on the robot model; other constraints were used depending on the desired motion: \begin{eqnarray} min \mbox{ } J = \Psi[q^p (t_f), \dot{q}^p (t_f), t_f] + \int_0^{t_f} L[x(t),u(t),t] dt \\ M(q)\ddot{q} + C(q,\dot{q}) + G(q) = \tau + \sum \Phi^T_q F \\ q^a(0) = q_0^a, \mbox{ } q^a(t_f) = q^a_f \\ q^p(0) = q^p_0, \mbox{ } \dot{q}^p(0) = \dot{q}^p_0 \\ \underline{q} < q < \overline{q} \\ \underline{\dot{q}} < \dot{q} < \overline{\dot{q}} \end{eqnarray} The matrix $\Phi^T_q$ is the Jacobian of the contact point, and $F$ is the external force applied at the contact point (the normal and/or friction forces, obtained from our contact model). The summation of $\Phi^T_q F$ represents the contribution of all the external forces applied to the system. We solve the optimal control problem with a direct method: guess a solution, integrate the equations of motion with the guessed solution, evaluate $J$, and use gradient information to correct the solution so that the cost decreases until a minimum value for $J$ is found. The guessed solution is the path of the active joints $q^{a}(t)$, which are parametrized with quintic B-splines. %\[ %q^a (t) = \sum_{i = 1}^{n} a_{i} B_{i,j} %\] %where $B_{i,j}$ are the spline basis functions, and $a_i$ are the %scaling coefficients. This Defining $q^{a}(t)$ also defines the active joint velocities and accelerations $\dot{q}^{a}(t), \ddot{q}^{a}(t)$. Because all the terms in the cost function are uniquely determined by the specified active joint accelerations $\ddot{q}^{a}(t)$ and the torque on the passive joints $\tau^{p}$, the problem can be transformed into one of static parameter optimization. The spline parameters defining $q^{a}(t)$ are the parameters that are varied during the optimization. In our C-STORM software, we have a recursive hybrid dynamics algorithm \cite{Sohl00}which solves the dynamics equations for the active joint torques $\tau^a$ and the passive joint accelerations $\ddot{q}^p$. This algorithm takes as input the current state ($q,\dot{q}$) along with the active joint accelerations $\ddot{q}^a$ and passive joint torques $\tau^p$. Let the recursive algorithm be referred to as $g$ with: \begin{equation} \left( \tau^a, \ddot{q}^p \right) = g(q,\dot{q},\ddot{q}^a,\tau^p). \label{g} \end{equation} Given an input $\ddot q^a(t),$ $\tau^p(t),$ and initial conditions $q(0),\dot{ q(0)},$ we use (\ref{g}) to solve the differential equation \begin{equation} \frac{d}{dt} \left\{\begin{array}{c} q^p \\ \dot{q}^p \\ \int_{0}^{t}{\frac{1}{2}||\psi||^2 dt} \end{array}\right\} = \left\{\begin{array}{c} \dot{q}^p \\ \ddot{q}^p(q,\dot{q},\ddot{q^a}, \tau^p) \\ \frac{1}{2}||\psi(q,\dot{q},\ddot{q^a}, \tau^p)||^2 \end{array}\right\} \label{eqn:sysint} \end{equation} for $q^p(t).$ The third equation was appended to the integration so that we could compute $\int_{0}^{t}{\frac{1}{2}||\psi||^2 dt,}$ which is used in the cost function for some of our example problems. The exact form of $\psi$ was different for the different cost functions. Once the system has been integrated forward in time, using this algorithm, the cost function can be evaluated, and then the derivative of the hybrid algorithm is used to get the gradient of the cost function. The derivative of the hybrid algorithm \cite{SohlBobrow99}; is used to solve for the active joint torques, the passive joint accelerations, and the derivatives of both of those with respect to all of the current joint positions, velocities, and accelerations. This algorithm takes the same inputs as $g$, with the additional inputs of the derivatives of the torques applied on the passive joints $\tau^p$ with respect to all of the current joint positions, velocities, and accelerations. Let the derivative of the recursive algorithm be referred to as $dg$ with: \begin{eqnarray} \left( \tau^a, \ddot{q}^p, \frac{\partial (\ddot{q}^p, \tau^a)}{\partial q}, \frac{\partial (\ddot{q}^p, \tau^a)}{\partial \dot{q}}, \frac{\partial (\ddot{q}^p, \tau^a)}{\partial \ddot{q}} \right) = \\ dg \left( q,\dot{q},\ddot{q}^a,\tau^p, \frac{\partial \tau^p}{\partial q}, \frac{\partial \tau^p}{\partial \dot{q}}, \frac{\partial \tau^p}{\partial \ddot{q}} \right) \nonumber \end{eqnarray} Because the $dg$ algorithm requires the inputs $\frac{\partial \tau^p}{\partial q}$, $\frac{\partial \tau^p}{\partial \dot{q}}$, and $\frac{\partial \tau^p}{\partial \ddot{q}}$, the applied forces to the robot from the environment must be continuous and continuously differentiable. The analytic gradient of the cost function can be calculated with the output from $dg$ and passed to MATLAB's BFGS SQP function that performs parameter optimization. With the cost function and its gradient, the spline parameters can be varied so as to find the local minimum value of $J$, and the corresponding controls. \section{Contact model, Normal and Fricion Forces} % i. normal and friction forces %---------------------------big snip from qual-------------------------- Contact between two bodies results in two forces, a force in the normal direction and a friction force in the direction tangential to the contact surface. In our contact model, these forces are modeled with virtual spring-damper systems at the point of contact, which requires knowing the distance between the two bodies, as well as their relative velocity expressed in an inertial reference frame. (For simplicity, any object in the environment including the ground will be referred to as an obstacle, and the obstacle is assumed to remain stationary.) Additionally, the forces are scaled by smoothing functions, to ensure that the forces are continuous and continuously differentiable, and that they begin at zero when contact is first made. These conditions are necessary for our dynamics algorithm to to calculate the analytic gradient correctly, as previously discussed. To formulate the general contact forces, we use a distance algorithm (is it Johnson and Gilbert's, by the way? so I can put the right reference here...) that steps through all the combination pairs made up of one shape from the robot and one shape from the obstacle. This distance algorithm finds the near points on each shape in the pair--that is, the point on shape A that is closest to shape B, and vice versa. The point on the robot shape closest to the obstacle is denoted $p_{rob}$, and the point on the obstacle shape closest to the robot is denoted $p_{obs}$ %(see Figure \ref{fig:gencoll}). %\begin{figure} %\centerline{\psfig{file=gencoll.eps,height=3.0in}} %\centerline{\psfig{file=gencoll.eps}} %% %\caption{Points, Vectors, and Unit Vectors used in General %Collision Model} %\label{fig:gencoll} %\end{figure} For each pair of shapes, the distance algorithm returns three objects: a vector expressed in the inertial frame that gives the location of the near point $p_{rob}$, a vector that begins at $p_{obs}$ and ends at $p_{rob}$ (also expressed in the inertial frame) called $d_{vec}$, and to which robot link the robot shape is attached. Taking the norm of $d_{vec}$ gives the distance $d$ between the robot and the obstacle shapes, which is one of the values to calculate the contact forces. The other necessary value, the velocity of $p_{rob}$, can be obtained using the Jacobian $J(q,p_{rob}$: \begin{equation} [\omega v]^T = \dot{x} = J(q,p_{rob}) \dot{q} \end{equation} where $q$ is the vector of robot joint angles, and $J$ is the Jacobian of the robot to point $p_{rob}$. %$V$ is the generalized %velocity $(w,v)$, so we pull the linear velocity $v$ of %$p_{rob}$ out of $V$. The linear velocity $v$ is expressed as components in the normal direction $\hat{u}_d$, and the tangential direction $\hat{u}_t$: \begin{equation} v = v_d + v_t. \end{equation} %Okay, start to talk...need to explain how it all works, which, %hah, I have managed to avoid until now... For the force model to generate a normal force, the virtual spring and damper used %For the normal %force, we need the component of $v$ in the normal %direction $\hat{u}_d$, defined as: %\begin{eqnarray} %\hat{u}_d = \frac{d_{vec}}{\| d_{vec}. %\end{eqnarray} % %and get the component of $v$ in the $\hat{u}_d$ direction %with the dot product: %\begin{eqnarray} %v_{normal} = v^T \hat{u}_d \\ %v_{normvec} = v_{normal} \hat{u}_d %\end{eqnarray} Calling the unstretched spring length $d_{on}$, with the above expressions the general normal force can be written: \begin{eqnarray} N = \beta (k(d_{on} - d) \hat{u}_d -c v_{normal} \hat{u}_d) \\ \beta (d) = \left\{ \begin{array}{ll} 0 & \mbox{if } d \geq d_{on} \\ a_0 + a_1 d + a_2 d^2 + a_3 d^3 & \mbox{if } d_{on} > d > d_{off} \\ 1 & \mbox{if } d_{off} \geq d \end{array} \right. \\ \beta(d_{on}) = 0, \mbox{ } \beta' (d_{on}) = 0 \\ \beta(d_{off}) = 1, \mbox{ } \beta' (d_{off}) = 0 \end{eqnarray} where the coefficients $(a_0,a_1,a_2,a_3)$ are written in terms of $d_{on},d_{off}$, and $\beta (d)$ ensures that the analytic gradients of the equations of motion are continuous. The constants $d_{on}$, $d_{off}$ are set by the user. %The general %contact force $N$ in the normal direction was modeled %with a spring-damper system, and is of the form: %\begin{equation} %N = \beta(k(x_0 - x) - c {\dot x}) %\end{equation} %where $x_0$ is the undeflected spring length, $k$ %is the spring constant, $c$ is the damper constant, %$x$ is the current length of the spring, ${\dot x}$ %is the velocity of the end of the spring, and $\beta(x)$ %is the smoothing function, to make the derivative of %$N$ $C^1$ . %%(see Figure \ref{fig:springdamper}). %%\begin{figure} %%\centerline{\psfig{file=springdamper.eps,height=2.0in}} %%% %%\caption{Virtual Spring-Damper System for Modeling Contact Forces} %%\label{fig:springdamper2} %%\end{figure} %The friction force $f_{fric}$ is %defined from the normal force, and has the form: %\begin{equation} %f_{fric} = -.3 N \gamma %\end{equation} %where $N$ is the normal force defined above and %$\gamma$ is the smoothing function for the %friction force. The purpose of $\beta,\gamma$ is %the same as before in the Luxo example, to %keep the derivatives and analytic gradients $C^1$, %but they are implemented differently for the %general case. For the friction force, the norm of $N$ is used in the expression for $f_{fric}$ and once $\gamma$ is defined, the general contact forces are completely specified. The unit vector $\hat{u}_t$ is defined using the tangential component $v_t$ of the linear velocity $v$, as follows: \begin{eqnarray} v_t = v - v_{normvec} \\ v_{t,norm} = \| v_t \| \\ v_{max} = 0.1 \\ \hat{u}_t = \frac{v_t}{v_{t,norm}} \end{eqnarray} $v_{max}$ is the value of the tangential velocity norm $v_{t,norm}$ at which the smoothing function $\gamma (v_{t,norm})$ goes to a value of one. $\hat{u}_t$ is used to define the direction in which the friction force acts. Together, these define the friction force as follows: \begin{eqnarray} f_{fric} = -.3N \gamma \hat{u}_t \\ \gamma (v_{t,norm}) = \left\{ \begin{array}{ll} 1 & \mbox{if } v_{t,norm} > v_{max} \\ \xi & \mbox{otherwise} %1.5(\frac{v_{t,norm}}{v_{max}}) - %0.5(\frac{v_{t,norm}}{v_{max}})^3 & \mbox{otherwise} \end{array} \right. \\ \xi = 1.5(\frac{v_{t,norm}}{v_{max}}) - 0.5(\frac{v_{t,norm}}{v_{max}})^3 \\ \gamma(0) = 0, \mbox{ } \gamma' (0) = 1 \\ \gamma(v_{max}) = 1, \mbox{ } \gamma' (v_{max}) = 0 \end{eqnarray} Now we need to write the derivatives of these forces. This will be described in the next section. \section{Derivative of Distance Functions} %---------------------------end big normal and friction force from qual snip % ii. derivative of distance functions %---------------------------big snip from qual-------------------------- There are additional derivatives that have to be calculated with the general contact/collision model. Because of the need for %${\partial \tau}/{\partial q}$, %${\partial \tau}/{\partial \dot{q}}$, %${\partial \tau}/{\partial \ddot{q}}$ the derivatives of the contact forces $N$ and $f_{fric}$, we need expressions for the derivative of the Jacobian with respect to the joint variables $q$, and the derivative of the near points $p_{rob},p_{obs}$ with respect to the joint variables $q$. The derivative ${\partial J}/{\partial q}$ is found as explained in chapter 2. For obtaining the derivatives $(\partial p_{rob}/\partial q,\partial p_{obs}/\partial q)$, an optimization problem is set up that defines the positions of the near points $(p_{rob}, p_{obs})$, and the derivative of the optimization problem with respect to $q$ is used to define the necessary derivatives \cite{Luenberger}. The equivalent optimization problem is set up as follows: \begin{eqnarray} \mbox{min } f(x) = \|p_{rob} - p_{obs} \|^2 \\ \mbox{subject to } h(x) = 0 \\ g(x) \leq 0 \end{eqnarray} where $f(x)$ is the objective function, $x$ is a vector of parameters that define the position of points on the shapes of the robot and obstacle, $h(x)$ are the equality constraints and $g(x)$ are the inequality constraints. In this case the constraints would ensure that the near points would be located on the surfaces of their respective shapes, and their specific form would depend on the particular shapes on the robot and obstacle. When inequality constraints are active they are equality constraints, and so the problem can be looked at as just having equality constraints (especially since we're not solving the optimization problem for the near points, we are simply setting it up to get expressions for the derivatives of the near points, so we know ahead of time which constraints are active). % The system of equations whose solution is the near points is: \begin{eqnarray} \nabla f(x^*) + {\lambda}^T \nabla h(x^*) = 0 \\ h(x^*) = b. \end{eqnarray} Depending on the shapes used to describe the robot and obstacles, this can be factored to get something in the form of: \begin{eqnarray} Qx - D^T \lambda = 0 \\ Ax = b, \end{eqnarray} where $Q,D$ and $A$ are functions of the robot position $q$. This can be rewritten as: \begin{equation} \left[ \begin{array}{cc} Q & -D^T \\ A & 0 \end{array} \right] \left[ \begin{array}{c} x \\ \lambda \end{array} \right] = M \left[ \begin{array}{c} x \\ \lambda \end{array} \right] = \left[ \begin{array}{c} 0 \\ b \end{array} \right] \end{equation} where the near points are the values $x$ from \begin{equation} \left[ \begin{array}{c} x \\ \lambda \end{array} \right] = M^{-1} \left[ \begin{array}{c} 0 \\ b \end{array} \right] \end{equation} To get the derivative of the near points with respect to the joint angles, we take the partial derivative of this expression with respect to the joint angles to get: \begin{equation} \frac{\partial M}{\partial q} \left[ \begin{array}{c} x \\ \lambda \end{array} \right] + M \left[ \begin{array}{c} \frac{\partial x}{\partial q} \\ \frac{\partial \lambda}{\partial q} \end{array} \right] = \left[ \begin{array}{c} 0 \\ \frac{\partial b}{\partial q} \end{array} \right] \end{equation} We have expressions for $M$ as functions of $q$, so $\partial M/\partial q$ is defined, we have $[x \mbox{ }\lambda]^T = M^{-1}[0 \mbox{ }b]^T$ from equation above, we have $M$, and we have $\partial b/\partial q$, so that the equation can be solved for $(\partial x/\partial q,\partial \lambda/\partial q)$, where $\partial x/\partial q$ is the expression we're seeking. $\partial x/\partial q$ encodes how the near points change due to how the surface of the shape changes as the robot moves through space. These equations have only been implemented for the relatively simple case of the Luxo, and not all possible shape combinations have been added to the software. \section{Simulation Results} \subsection{Up to a Handstand} \subsection{From Handstand to Roll} \subsection{From Roll Back To Standing up} \subsection{From standing to start position} \section{Conclusion} %this is how to do an unnumbered subsection \subsection*{Acknowledgments} This is how to do an unnumbered subsection, which comes out in 11 point bold font. Here I thank my colleagues, especially Mike Gennert, who know more about Tex and LaTeX than I. %\begin{thebibliography}{9} %\bibitem{key:foo} %I. M. Author, %``Some Related Article I Wrote,'' %{\em Some Fine Journal}, Vol. 17, pp. 1-100, 1987. %\end{thebibliography} \end{document} %---------------------------------------------------------------------- %Outline: % %I. Introduction % Our goal starting out is to generate motion segments for a robot. So we %start from a desire to do robot path planning in some intelligent way. %And because we are interested in robots that make and break contact with the %ground (like hopping robots, walking robots, etc.) we need to be able to %deal with those types of situations. So: % %i. path planning for a robot % % %ii. do with optimal control approach % a. explain why we chose to go with the optimal control approach % i. don't need tons of a priori information, can make more % high-level specifications of what motion we want to see... % ii. can use to find motions that allow you to use a resource % like fuel most efficiently, or to extend the capabilities of % the robot (like lifting more than the rated payload...) % b. explain that the optimal control approach requires certain things % to converge reliably, like $C^2$ functions for everything in the model % i. explain why that is? % ii. implication for however we model the contact -- all the functions % used in the equations of motion have to be $C^2$ % %-> Is this really a sub-part of optimal control, or -- no, wouldn't we have %to make this decision, regardless of how we were choosing to generate the %actual motions? %iii. System model % c. explain that there are two ways to go with the model % (just to model the dynamics) % i. switch models for different phases of the motion % ii. one model throughout and model contact forces from environment % as outside forces acting on the model % d. explain we do the the one model throughout % i. why we do it this way % ii. implications for our contact model % %Anyway, we end up with two things to worry about: because of the types of %motions we want to look for/work with, we need a contact model, and because %of our approach to generating motion segments, we need that contact model %to involve functions that are $C^2$. So: % %--what is our contact model? % --background information, and where to find the physical justification % for the many magical things we do... % (Tenaglia and Orin, and their refs, etc.) % --how we do the normal and friction forces % %To model the collision forces %on the robot, we have to be able to determine the distances between %surfaces of the robot and other objects, the location of the near points %on the robot and environmental objects, and the derivative that describes %how these points move as the robot moves. We have an approach that allows %us to describe the necessary distance derivatives so that the collision %forces and their derivatives are continuous and defined. % %%Maybe this stuff doesn't need to be here, except for in the paragraph at %%the end of the introduction that works as a road map for the rest of the %%paper %%--how we generate motions %%--how we put the motions together currently... % %What is the main thrust of this paper -- we wanted to find motion segments %for robots that make and break contact with the ground, so we decided on %this particular optimal control approach, and this form of system model, which %led to us having this kind of contact model. This paper details the %contact model, describes how we find the derivative of distances between %the robot and its environment, %outlines our approach to obtaining motion segments, and %shows the results of a series of optimizations done with the tumbler robot. %