\n"," ![]()
\n","
\n","\n","\n","\n"],"text/plain":["\n"," \n"," \n"," \n","
\n","![](\"https://raw.githubusercontent.com/RussTedrake/underactuated/master/figures/exercises/compass_gait.jpg\")
\n","\n","The idea is simple: we need to find an initial state so that, after a walking cycle, the robot comes back exactly to the same state.\n","Since the robot is completely symmetric (the legs have the same length and mass distribution), there is no need to optimize for the whole walking cycle (two steps).\n","We just optimize a single step, then we \"mirror\" the result to obtain the second step and complete the walking cycle (see the figure below).\n","\n","![](\"https://raw.githubusercontent.com/RussTedrake/underactuated/master/figures/exercises/compass_gait_configurations.jpg\")
"]},{"cell_type":"markdown","metadata":{"id":"Kb4nmcdXUOEo"},"source":["## The Walking Cycle\n","\n","At the initial time $t=0$, we require the robot to start its motion with both the feet on the ground (figure on the left above).\n","\n","The stance foot must be in contact with the ground for all times ($y(t)=0$), while the swing foot is allowed to break contact with the ground but not to penetrate it.\n","The contact force at the stance foot must always lie in the friction cone.\n","\n","To move from the initial to the final configurations above, the compass gait must necessarily scuff the swing foot on the ground at some time between $t=0$ and $t=t_c$.\n","In our model we assume that this scuffing does not generate any contact force.\n","\n","At the end of the step (right figure), the swing foot must collide (i.e. be in contact) with the ground: this moment is called the \"heel strike\" and denoted as $t_c$.\n","At the heel strike we have an impulse on the swing foot which must also lie in the friction cone ($\\int_{t_c^-}^{t_c^+} \\lambda dt$ [in the textbook appendix](http://underactuated.csail.mit.edu/multibody.html#impulsive_collision)).\n","This impulse leads to a jump in the system state as described [in the lecture notes](http://underactuated.mit.edu/simple_legs.html#compass_gait).\n","\n","The periodicity of the motion can be enforced as follows:\n","- Periodicity of the configuration $\\mathbf{q}$.\n","We want the final angles of the legs to be equal to the initial one, but with reversed signs: $\\theta_1(t_c) = \\theta_1(0)$ for the stance leg, and $\\theta_2(t_c) = - \\theta_2(0)$ for the swing leg.\n","- Periodicity of the velocity $\\dot{\\mathbf{q}}$.\n","Since the heel strike is inelastic, after the impact the swing foot must have zero translational velocity.\n","Hence, to enforce periodicity, we require $\\dot{x}(0) = \\dot{y}(0) = 0$.\n","The (absolute) angular velocity of the swing leg at time $t=t_c^+$ must be equal to the one of the stance leg at time $t=0$: some simple kinematics shows that this can be enforced as $\\dot{\\theta}_1(0) = \\dot{\\theta}_1(t_c^+) + \\dot{\\theta}_2(t_c^+)$.\n","Finally, to ensure that the relative velocity of the legs is periodic, we simply enforce $\\dot{\\theta}_2(0) = - \\dot{\\theta}_2(t_c^+)$."]},{"cell_type":"markdown","metadata":{"id":"sI95iB7cUOEq"},"source":["## Parse the `urdf` and Get the `MultibodyPlant`\n","\n","We start by defining a couple of physical parameters that we will need below."]},{"cell_type":"code","metadata":{"id":"V8zGA3NUUOEq","executionInfo":{"status":"ok","timestamp":1619218255003,"user_tz":240,"elapsed":360,"user":{"displayName":"Manmeet Bhabra","photoUrl":"","userId":"11686790358753401934"}}},"source":["# friction coefficient between feet and ground\n","friction = .2\n","\n","# position of the feet in the respective leg frame\n","# (must match the urdf)\n","foot_in_leg = {\n"," 'stance_leg': np.zeros(3), # stance foot in stance-leg frame\n"," 'swing_leg': np.array([0, 0, -1]) # swing foot in swing-leg frame\n","}"],"execution_count":115,"outputs":[]},{"cell_type":"markdown","metadata":{"id":"V9b02lw0UOEq"},"source":["Here we parse the `urdf` file to get a Drake `MultibodyPlant`.\n","\n","**The swing-foot scuffing.**\n","In this notebook we will use the compass gait `MultibodyPlant` only as a support for the computation of the robot kinematics and dynamics.\n","In particular, we will not use it for simulation.\n","The reason for this resides in the scuffing issue described above: when the swing foot is sliding on the ground at zero height, any good physic simulator detects a collision and applies a friction force to the foot.\n","This would cause the compass gait ot stumble and fall down, no matter the trajectory we found.\n","Note however that more complex robots, such as the [kneed walker](http://underactuated.mit.edu/simple_legs.html#kneed_walker), do not have this issue, and, for these, the workflow we use in this notebook can be used both for the detection of the limit cycle and for simulation.\n","\n","**An important implementation detail.**\n","Behind the scenes, the optimization solvers we use require the knowledge of the derivaties of the cost function and the constraint values with respect to the decision variables.\n","This is needed to understand in which direction the current solution should be corrected to find a feasible point or reduce the cost.\n","This process used to be very tedious some years ago, when graduate students had to spend many hours writing down these derivatives \"by hand\".\n","Nowadays, we use [automatic differentiation](https://en.wikipedia.org/wiki/Automatic_differentiation), which through the construction of a computational graph is able to evaluate a function and its derivatives very quickly and exactly (cf. [finite difference](https://en.wikipedia.org/wiki/Finite_difference)).\n","To allow the evaluation of the `MultibodyPlant` functions (e.g. the mass matrix method) with `AutoDiffXd` variables, we need to call the `MultibodyPlant.ToAutoDiffXd()` function which returns a copy of the `MultibodyPlant` that can work with autodiff variables."]},{"cell_type":"code","metadata":{"id":"5-YD6GhfUOEr","executionInfo":{"status":"ok","timestamp":1619218255319,"user_tz":240,"elapsed":369,"user":{"displayName":"Manmeet Bhabra","photoUrl":"","userId":"11686790358753401934"}}},"source":["# parse urdf and create the MultibodyPlant\n","compass_gait = MultibodyPlant(time_step=0)\n","file_name = FindResource('models/compass_gait_limit_cycle.urdf')\n","Parser(compass_gait).AddModelFromFile(file_name)\n","compass_gait.Finalize()\n","\n","# overwrite MultibodyPlant with its autodiff copy\n","compass_gait = compass_gait.ToAutoDiffXd()\n","\n","# number of configuration variables\n","nq = compass_gait.num_positions()\n","\n","# number of components of the contact forces\n","nf = 2"],"execution_count":116,"outputs":[]},{"cell_type":"markdown","metadata":{"id":"F5Zv2GW4UOEr"},"source":["## Helper Functions for the `MathematicalProgram`\n","\n","When writing a `MathematicalProgram` in Drake, optimization variables are `symbolic.Variable` objects.\n","These cannot be passed directly to the `MultibodyPlant` functions (such as `CalcMassMatrix`), which only accept floats or `AutoDiffXd` types.\n","Hence, if you need to add a constraint which involves the evaluation of a `MultibodyPlant` function, you need to proceed as follows.\n","\n","Write a python function (say `my_fun`) that, given the numeric value (`float` or `AutoDiffXd`) of certain variables in the problem, returns the numeric value of the quantity that you want to constrain.\n","Let `vars` be the arguments of this function and `values` its output (both can be arrays).\n","Using the method `MathematicalProgram.AddConstraint` you can write `prog.AddConstraint(my_fun, lb=values_lb, ub=values_ub, vars=vars)` to enforce the constraints `values_lb <= values <= values_ub`, where `value_lb` and `value_ub` are vectors of floats of appropriate dimensions.\n","Then, at solution time, the solver will evaluate `my_fun` passing autodiff variables, retrieving in this way the numeric values of the constraint violations and their derivatives.\n","\n","In the following cell we wrote the functions that we will need to enforce the necessary constraints in the trajectory optimization problem."]},{"cell_type":"code","metadata":{"id":"ejb2F2JUUOEs","executionInfo":{"status":"ok","timestamp":1619218255626,"user_tz":240,"elapsed":399,"user":{"displayName":"Manmeet Bhabra","photoUrl":"","userId":"11686790358753401934"}}},"source":["# Function that given the current configuration, velocity,\n","# acceleration, and contact force at the stance foot, evaluates\n","# the manipulator equations. The output of this function is a\n","# vector with dimensions equal to the number of configuration\n","# variables. If the output of this function is equal to zero\n","# then the given arguments verify the manipulator equations.\n","def manipulator_equations(vars):\n"," \n"," # split input vector in subvariables\n"," # configuration, velocity, acceleration, stance-foot force\n"," assert vars.size == 3 * nq + nf\n"," split_at = [nq, 2 * nq, 3 * nq]\n"," q, qd, qdd, f = np.split(vars, split_at)\n"," \n"," # set compass gait state\n"," context = compass_gait.CreateDefaultContext()\n"," compass_gait.SetPositions(context, q)\n"," compass_gait.SetVelocities(context, qd)\n"," \n"," # matrices for the manipulator equations\n"," M = compass_gait.CalcMassMatrixViaInverseDynamics(context)\n"," Cv = compass_gait.CalcBiasTerm(context)\n"," tauG = compass_gait.CalcGravityGeneralizedForces(context)\n"," \n"," # Jacobian of the stance foot\n"," J = get_foot_jacobian(compass_gait, context, 'stance_leg')\n"," \n"," # return violation of the manipulator equations\n"," return M.dot(qdd) + Cv - tauG - J.T.dot(f)\n","\n","# Function that given the current configuration, returns\n","# the distance of the swing foot from the ground (scalar).\n","# We have penetration if the function output is negative.\n","def swing_foot_height(q):\n"," \n"," # get reference frames for the swing leg and the ground\n"," leg_frame = compass_gait.GetBodyByName('swing_leg').body_frame()\n"," ground_frame = compass_gait.GetBodyByName('ground').body_frame()\n"," \n"," # position of the swing foot in ground coordinates\n"," context = compass_gait.CreateDefaultContext()\n"," compass_gait.SetPositions(context, q)\n"," swing_foot_position = compass_gait.CalcPointsPositions(\n"," context,\n"," leg_frame,\n"," foot_in_leg['swing_leg'],\n"," ground_frame\n"," )\n"," \n"," # return only the coordinate z\n"," # (distance normal to the ground)\n"," return swing_foot_position[-1]\n","\n","# Function that implements the impulsive collision derived in\n","# the textbook appendix. Arguments are: compass gait configuration,\n","# velocities before and after heel strike, and the swing-foot\n","# impulse (in latex, $\\int_{t_c^-}^{t_c^+} \\lambda dt$).\n","# Returns a vector of quantities that must vanish in order\n","# for the impulsive dynamics to be verified: it enforces the velocity\n","# jump due to the impulse, and the inelastic behavior of the\n","# collision (zero coefficient of restitution $e$).\n","# See http://underactuated.mit.edu/multibody.html#impulsive_collision\n","def reset_velocity_heelstrike(vars):\n"," \n"," # split input vector in subvariables\n"," # qd_pre: generalized velocity before the heel strike\n"," # qd_post: generalized velocity after the heel strike\n"," # imp: swing-foot collision impulse (2d vector)\n"," assert vars.size == 3 * nq + nf\n"," split_at = [nq, 2 * nq, 3 * nq]\n"," q, qd_pre, qd_post, imp = np.split(vars, split_at)\n","\n"," # set compass gait configuration\n"," context = compass_gait.CreateDefaultContext()\n"," compass_gait.SetPositions(context, q)\n"," \n"," # get necessary matrices\n"," M = compass_gait.CalcMassMatrixViaInverseDynamics(context)\n"," J = get_foot_jacobian(compass_gait, context, 'swing_leg')\n"," \n"," # return a vector that must vanish for the impulsive dynamics to hold\n"," return np.concatenate((\n"," M.dot(qd_post - qd_pre) - J.T.dot(imp), # velocity jump due to the impulse\n"," J.dot(qd_post) # zero velocity restitution\n"," ))\n","\n","# Function that given a leg, returns the Jacobian matrix for the related foot.\n","def get_foot_jacobian(compass_gait, context, leg):\n"," \n"," # get reference frames for the given leg and the ground\n"," leg_frame = compass_gait.GetBodyByName(leg).body_frame()\n"," ground_frame = compass_gait.GetBodyByName('ground').body_frame()\n","\n"," # compute Jacobian matrix\n"," J = compass_gait.CalcJacobianTranslationalVelocity(\n"," context,\n"," JacobianWrtVariable(0),\n"," leg_frame,\n"," foot_in_leg[leg],\n"," ground_frame,\n"," ground_frame\n"," )\n"," \n"," # discard y components since we are in 2D\n"," return J[[0, 2]]"],"execution_count":117,"outputs":[]},{"cell_type":"markdown","metadata":{"id":"2u5EGPo8UOEs"},"source":["## The Trajectory Optimization Problem\n","\n","We start by setting some parameters of our optimization problem."]},{"cell_type":"code","metadata":{"id":"26KAM1vOUOEt","executionInfo":{"status":"ok","timestamp":1619218255765,"user_tz":240,"elapsed":217,"user":{"displayName":"Manmeet Bhabra","photoUrl":"","userId":"11686790358753401934"}}},"source":["# time steps in the trajectory optimization\n","T = 50\n","\n","# minimum and maximum time interval is seconds\n","h_min = .005\n","h_max = .05"],"execution_count":118,"outputs":[]},{"cell_type":"markdown","metadata":{"id":"uM_S4BhiUOEt"},"source":["**Troubleshooting.**\n","To simplify the reading, we divide the construction of the `MathematicalProgram` in multiple cells.\n","If you modify any of the components of the problem, be sure to rerun your code starting from the following cell (where the `MathematicalProgram` is initialized).\n","Otherwise you will add the same constraints multiple times to the same optimization problem.\n","\n","We start from the decision variables of the trajectory optimization problem.\n","Notice that we also add the accelerations `qdd` among the optimization variables here.\n","This is slightly unusual, and not necessary, but in these circumstances it simplifies a bit the code."]},{"cell_type":"code","metadata":{"id":"EJ0p7YRcUOEt","executionInfo":{"status":"ok","timestamp":1619218256249,"user_tz":240,"elapsed":398,"user":{"displayName":"Manmeet Bhabra","photoUrl":"","userId":"11686790358753401934"}}},"source":["# initialize program\n","prog = MathematicalProgram()\n","\n","# vector of the time intervals\n","# (distances between the T + 1 break points)\n","h = prog.NewContinuousVariables(T, name='h')\n","\n","# system configuration, generalized velocities, and accelerations\n","q = prog.NewContinuousVariables(rows=T+1, cols=nq, name='q')\n","qd = prog.NewContinuousVariables(rows=T+1, cols=nq, name='qd')\n","qdd = prog.NewContinuousVariables(rows=T, cols=nq, name='qdd')\n","\n","# stance-foot force\n","f = prog.NewContinuousVariables(rows=T, cols=nf, name='f')\n","\n","# heel strike impulse for the swing leg\n","imp = prog.NewContinuousVariables(nf, name='imp')\n","\n","# generalized velocity after the heel strike\n","# (if \"mirrored\", this velocity must coincide with the\n","# initial velocity qd[0] to ensure periodicity)\n","qd_post = prog.NewContinuousVariables(nq, name='qd_post')"],"execution_count":119,"outputs":[]},{"cell_type":"markdown","metadata":{"id":"6tgZZtJEUOEu"},"source":["Here are part of the constraints of the optimization problem."]},{"cell_type":"code","metadata":{"id":"0sDOEaS7UOEu","colab":{"base_uri":"https://localhost:8080/"},"executionInfo":{"status":"ok","timestamp":1619218256552,"user_tz":240,"elapsed":389,"user":{"displayName":"Manmeet Bhabra","photoUrl":"","userId":"11686790358753401934"}},"outputId":"5f97c2cd-db1f-4f5f-e204-d2f8155de719"},"source":["# lower and upper bound on the time steps for all t\n","prog.AddBoundingBoxConstraint([h_min] * T, [h_max] * T, h)\n","\n","# link the configurations, velocities, and accelerations\n","# uses implicit Euler method, https://en.wikipedia.org/wiki/Backward_Euler_method\n","for t in range(T):\n"," prog.AddConstraint(eq(q[t+1], q[t] + h[t] * qd[t+1]))\n"," prog.AddConstraint(eq(qd[t+1], qd[t] + h[t] * qdd[t]))\n","\n","# manipulator equations for all t (implicit Euler)\n","for t in range(T):\n"," vars = np.concatenate((q[t+1], qd[t+1], qdd[t], f[t]))\n"," prog.AddConstraint(manipulator_equations, lb=[0]*nq, ub=[0]*nq, vars=vars)\n"," \n","# velocity reset across heel strike\n","# see http://underactuated.mit.edu/multibody.html#impulsive_collision\n","vars = np.concatenate((q[-1], qd[-1], qd_post, imp))\n","prog.AddConstraint(reset_velocity_heelstrike, lb=[0]*(nq+nf), ub=[0]*(nq+nf), vars=vars)\n"," \n","# mirror initial and final configuration\n","# see \"The Walking Cycle\" section of this notebook\n","prog.AddLinearConstraint(eq(q[0], - q[-1]))\n","\n","# mirror constraint between initial and final velocity\n","# see \"The Walking Cycle\" section of this notebook\n","prog.AddLinearConstraint(qd[0, 0] == 0)\n","prog.AddLinearConstraint(qd[0, 1] == 0)\n","prog.AddLinearConstraint(qd[0, 2] == qd_post[2] + qd_post[3])\n","prog.AddLinearConstraint(qd[0, 3] == - qd_post[3])"],"execution_count":120,"outputs":[{"output_type":"execute_result","data":{"text/plain":["