{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "This notebook provides examples to go along with the [textbook](http://underactuated.csail.mit.edu/humanoids.html). I recommend having both windows open, side-by-side!\n", "\n", "[Click here](http://underactuated.csail.mit.edu/drake.html#notebooks) for instructions on how to run the notebook on Deepnote and/or Google Colab." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "from functools import partial\n", "\n", "import numpy as np\n", "from pydrake.all import (AddMultibodyPlantSceneGraph,\n", " AddUnitQuaternionConstraintOnPlant, AutoDiffXd,\n", " DiagramBuilder, ExtractGradient, ExtractValue,\n", " InitializeAutoDiff, JacobianWrtVariable, JointIndex,\n", " MathematicalProgram, MeshcatVisualizerCpp,\n", " MultibodyPlant, OrientationConstraint, Parser,\n", " PidController, PiecewisePolynomial,\n", " PositionConstraint, RigidTransform, RotationMatrix,\n", " Simulator, SnoptSolver, Solve, StartMeshcat, eq,\n", " namedview, set_log_level)\n", "\n", "from underactuated import FindResource, running_as_notebook\n", "\n", "#set_log_level('off');\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Start the visualizer (run this cell only once, each instance consumes a port)\n", "meshcat = StartMeshcat()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# The LittleDog Robot\n", "\n", "[LittleDog](https://www.youtube.com/watch?v=QID_czp_fRg) is a robot built by BostonDynamics, programmed by teams competing in the DARPA Learning Locomotion program.\n", "\n", "# Simulation: Standing with PID Control" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "def set_home(plant, context):\n", " hip_roll = .1;\n", " hip_pitch = 1;\n", " knee = 1.55;\n", " plant.GetJointByName(\"front_right_hip_roll\").set_angle(context, -hip_roll)\n", " plant.GetJointByName(\"front_right_hip_pitch\").set_angle(context, hip_pitch)\n", " plant.GetJointByName(\"front_right_knee\").set_angle(context, -knee)\n", " plant.GetJointByName(\"front_left_hip_roll\").set_angle(context, hip_roll)\n", " plant.GetJointByName(\"front_left_hip_pitch\").set_angle(context, hip_pitch)\n", " plant.GetJointByName(\"front_left_knee\").set_angle(context, -knee)\n", " plant.GetJointByName(\"back_right_hip_roll\").set_angle(context, -hip_roll)\n", " plant.GetJointByName(\"back_right_hip_pitch\").set_angle(context, -hip_pitch)\n", " plant.GetJointByName(\"back_right_knee\").set_angle(context, knee)\n", " plant.GetJointByName(\"back_left_hip_roll\").set_angle(context, hip_roll)\n", " plant.GetJointByName(\"back_left_hip_pitch\").set_angle(context, -hip_pitch)\n", " plant.GetJointByName(\"back_left_knee\").set_angle(context, knee)\n", " plant.SetFreeBodyPose(context, plant.GetBodyByName(\"body\"), RigidTransform([0, 0, 0.146]))\n", "\n", "def run_pid_control():\n", " builder = DiagramBuilder()\n", " plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-3)\n", " parser = Parser(plant)\n", " parser.AddModelFromFile(FindResource('models/littledog/LittleDog.urdf'))\n", " parser.AddModelFromFile(FindResource('models/littledog/ground.urdf'))\n", " plant.Finalize()\n", "\n", " # Add a PD Controller\n", " kp = 1*np.ones(12)\n", " ki = 0.0*np.ones(12)\n", " kd = .1*np.ones(12)\n", " # Select the joint states (and ignore the floating-base states)\n", " S = np.zeros((24, 37))\n", " j = 0\n", " for i in range(plant.num_joints()):\n", " joint = plant.get_joint(JointIndex(i))\n", " if joint.num_positions() != 1:\n", " continue\n", " S[j, joint.position_start()] = 1\n", " S[12+j, joint.velocity_start()] = 1\n", " # use lower gain for the knee joints\n", " if \"knee\" in joint.name():\n", " kd[j] = 0.1\n", " j = j+1\n", "\n", " control = builder.AddSystem(PidController(\n", " kp=kp, ki=ki, kd=kd,\n", " state_projection=S,\n", " output_projection=plant.MakeActuationMatrix()[6:,:].T))\n", "\n", " builder.Connect(plant.get_state_output_port(), control.get_input_port_estimated_state())\n", " builder.Connect(control.get_output_port(), plant.get_actuation_input_port())\n", "\n", " visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph,\n", " meshcat)\n", "\n", " diagram = builder.Build()\n", " simulator = Simulator(diagram)\n", " context = simulator.get_mutable_context()\n", " plant_context = plant.GetMyContextFromRoot(context)\n", " set_home(plant, plant_context)\n", " x0 = S @ plant.get_state_output_port().Eval(plant_context)\n", " control.get_input_port_desired_state().FixValue(\n", " control.GetMyContextFromRoot(context), x0)\n", "\n", " simulator.set_target_realtime_rate(1.0 if running_as_notebook else 0)\n", " visualizer.StartRecording()\n", " simulator.AdvanceTo(3.0)\n", " visualizer.PublishRecording()\n", "\n", "run_pid_control()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Gait optimization\n", "\n", "This is my reimplementation of some results I initially implemented for \n", "`Hongkai Dai, Andrés Valenzuela, and Russ Tedrake. Whole-body motion planning with centroidal dynamics and full kinematics. IEEE-RAS International Conference on Humanoid Robots, 2014.` [.pdf](http://groups.csail.mit.edu/robotics-center/public_papers/Dai14.pdf) [video](https://www.youtube.com/watch?v=l3TEnNAyjmg)\n", "\n", "Note: In an old version of Drake, we wrapped the common functionality into [ComDynamicsFullKinematicsPlanner](https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/solvers/trajectoryOptimization/ComDynamicsFullKinematicsPlanner.m). I reconstructed that functionality explicitly here. (But it would be fun to get it back into the new Drake).\n", "\n", "It is still a work in progress (I haven't finished re-implementing all of the constraints, so the motions aren't dynamically feasible yet). I'll finish it soon!\n", "\n", "I used the following diagram to help think about the foot timings for the different gaits:\n", "![](https://upload.wikimedia.org/wikipedia/commons/c/cf/Gait_graphs.jpg)\n", "\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "tags": [] }, "outputs": [], "source": [ "# Need this because a==b returns True even if a = AutoDiffXd(1, [1, 2]), b= AutoDiffXd(2, [3, 4])\n", "# That's the behavior of AutoDiffXd in C++, also.\n", "def autoDiffArrayEqual(a,b):\n", " return np.array_equal(a, b) and np.array_equal(ExtractGradient(a), ExtractGradient(b))\n", "\n", "# TODO: promote this to drake (and make a version with model_instance)\n", "def MakeNamedViewPositions(mbp, view_name):\n", " names = [None]*mbp.num_positions()\n", " for ind in range(mbp.num_joints()):\n", " joint = mbp.get_joint(JointIndex(ind))\n", " # TODO: Handle planar joints, etc.\n", " assert(joint.num_positions() == 1)\n", " names[joint.position_start()] = joint.name()\n", " for ind in mbp.GetFloatingBaseBodies():\n", " body = mbp.get_body(ind)\n", " start = body.floating_positions_start()\n", " body_name = body.name()\n", " names[start] = body_name+'_qw'\n", " names[start+1] = body_name+'_qx'\n", " names[start+2] = body_name+'_qy'\n", " names[start+3] = body_name+'_qz'\n", " names[start+4] = body_name+'_x'\n", " names[start+5] = body_name+'_y'\n", " names[start+6] = body_name+'_z'\n", " return namedview(view_name, names)\n", "\n", "def MakeNamedViewVelocities(mbp, view_name):\n", " names = [None]*mbp.num_velocities()\n", " for ind in range(mbp.num_joints()):\n", " joint = mbp.get_joint(JointIndex(ind))\n", " # TODO: Handle planar joints, etc.\n", " assert(joint.num_velocities() == 1)\n", " names[joint.velocity_start()] = joint.name()\n", " for ind in mbp.GetFloatingBaseBodies():\n", " body = mbp.get_body(ind)\n", " start = body.floating_velocities_start() - mbp.num_positions()\n", " body_name = body.name()\n", " names[start] = body_name+'_wx'\n", " names[start+1] = body_name+'_wy'\n", " names[start+2] = body_name+'_wz'\n", " names[start+3] = body_name+'_vx'\n", " names[start+4] = body_name+'_vy'\n", " names[start+5] = body_name+'_vz'\n", " return namedview(view_name, names)\n", "\n", "def gait_optimization(gait = 'walking_trot'):\n", " builder = DiagramBuilder()\n", " plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-3)\n", " parser = Parser(plant)\n", " littledog = parser.AddModelFromFile(FindResource('models/littledog/LittleDog.urdf'))\n", " plant.Finalize()\n", " visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph,\n", " meshcat)\n", "\n", " diagram = builder.Build()\n", " context = diagram.CreateDefaultContext()\n", " plant_context = plant.GetMyContextFromRoot(context)\n", " set_home(plant, plant_context)\n", " diagram.Publish(context)\n", "\n", " q0 = plant.GetPositions(plant_context)\n", " body_frame = plant.GetFrameByName(\"body\")\n", "\n", " PositionView = MakeNamedViewPositions(plant, \"Positions\")\n", " VelocityView = MakeNamedViewVelocities(plant, \"Velocities\")\n", "\n", " mu = 1 # rubber on rubber\n", " total_mass = sum(plant.get_body(index).get_mass(context) for index in plant.GetBodyIndices(littledog))\n", " gravity = plant.gravity_field().gravity_vector()\n", "\n", " nq = 12\n", " foot_frame = [\n", " plant.GetFrameByName('front_left_foot_center'),\n", " plant.GetFrameByName('front_right_foot_center'),\n", " plant.GetFrameByName('back_left_foot_center'),\n", " plant.GetFrameByName('back_right_foot_center')]\n", "\n", " # setup gait\n", " is_laterally_symmetric = False\n", " check_self_collision = False\n", " if gait == 'running_trot':\n", " N = 21\n", " in_stance = np.zeros((4, N))\n", " in_stance[1, 3:17] = 1\n", " in_stance[2, 3:17] = 1\n", " speed = 0.9\n", " stride_length = .55\n", " is_laterally_symmetric = True\n", " elif gait == 'walking_trot':\n", " N = 21\n", " in_stance = np.zeros((4, N))\n", " in_stance[0, :11] = 1\n", " in_stance[1, 8:N] = 1\n", " in_stance[2, 8:N] = 1\n", " in_stance[3, :11] = 1\n", " speed = 0.4\n", " stride_length = .25\n", " is_laterally_symmetric = True\n", " elif gait == 'rotary_gallop':\n", " N = 41\n", " in_stance = np.zeros((4, N))\n", " in_stance[0, 7:19] = 1\n", " in_stance[1, 3:15] = 1\n", " in_stance[2, 24:35] = 1\n", " in_stance[3, 26:38] = 1\n", " speed = 1\n", " stride_length = .65\n", " check_self_collision = True\n", " elif gait == 'bound':\n", " N = 41\n", " in_stance = np.zeros((4, N))\n", " in_stance[0, 6:18] = 1\n", " in_stance[1, 6:18] = 1\n", " in_stance[2, 21:32] = 1\n", " in_stance[3, 21:32] = 1\n", " speed = 1.2\n", " stride_length = .55\n", " check_self_collision = True\n", " else:\n", " raise RuntimeError('Unknown gait.')\n", "\n", "\n", " T = stride_length / speed\n", " if is_laterally_symmetric:\n", " T = T / 2.0\n", "\n", " prog = MathematicalProgram()\n", "\n", " # Time steps\n", " h = prog.NewContinuousVariables(N-1, \"h\")\n", " prog.AddBoundingBoxConstraint(0.5*T/N, 2.0*T/N, h)\n", " prog.AddLinearConstraint(sum(h) >= .9*T)\n", " prog.AddLinearConstraint(sum(h) <= 1.1*T)\n", "\n", " # Create one context per timestep (to maximize cache hits)\n", " context = [plant.CreateDefaultContext() for i in range(N)]\n", " # We could get rid of this by implementing a few more Jacobians in MultibodyPlant:\n", " ad_plant = plant.ToAutoDiffXd()\n", "\n", " # Joint positions and velocities\n", " nq = plant.num_positions()\n", " nv = plant.num_velocities()\n", " q = prog.NewContinuousVariables(nq, N, \"q\")\n", " v = prog.NewContinuousVariables(nv, N, \"v\")\n", " q_view = PositionView(q)\n", " v_view = VelocityView(v)\n", " q0_view = PositionView(q0)\n", " # Joint costs\n", " q_cost = PositionView([1]*nq)\n", " v_cost = VelocityView([1]*nv)\n", " q_cost.body_x = 0\n", " q_cost.body_y = 0\n", " q_cost.body_qx = 0\n", " q_cost.body_qy = 0\n", " q_cost.body_qz = 0\n", " q_cost.body_qw = 0\n", " q_cost.front_left_hip_roll = 5\n", " q_cost.front_right_hip_roll = 5\n", " q_cost.back_left_hip_roll = 5\n", " q_cost.back_right_hip_roll = 5\n", " v_cost.body_vx = 0\n", " v_cost.body_wx = 0\n", " v_cost.body_wy = 0\n", " v_cost.body_wz = 0\n", " for n in range(N):\n", " # Joint limits\n", " prog.AddBoundingBoxConstraint(plant.GetPositionLowerLimits(), plant.GetPositionUpperLimits(), q[:,n])\n", " # Joint velocity limits\n", " prog.AddBoundingBoxConstraint(plant.GetVelocityLowerLimits(), plant.GetVelocityUpperLimits(), v[:,n])\n", " # Unit quaternions\n", " AddUnitQuaternionConstraintOnPlant(plant, q[:,n], prog)\n", " # Body orientation\n", " prog.AddConstraint(OrientationConstraint(plant,\n", " body_frame, RotationMatrix(),\n", " plant.world_frame(), RotationMatrix(),\n", " 0.1, context[n]), q[:,n])\n", " # Initial guess for all joint angles is the home position\n", " prog.SetInitialGuess(q[:,n], q0) # Solvers get stuck if the quaternion is initialized with all zeros.\n", "\n", " # Running costs:\n", " prog.AddQuadraticErrorCost(np.diag(q_cost), q0, q[:,n])\n", " prog.AddQuadraticErrorCost(np.diag(v_cost), [0]*nv, v[:,n])\n", "\n", " # Make a new autodiff context for this constraint (to maximize cache hits)\n", " ad_velocity_dynamics_context = [ad_plant.CreateDefaultContext() for i in range(N)]\n", " def velocity_dynamics_constraint(vars, context_index):\n", " h, q, v, qn = np.split(vars, [1, 1+nq, 1+nq+nv])\n", " if isinstance(vars[0], AutoDiffXd):\n", " if not autoDiffArrayEqual(q, ad_plant.GetPositions(ad_velocity_dynamics_context[context_index])):\n", " ad_plant.SetPositions(ad_velocity_dynamics_context[context_index], q)\n", " v_from_qdot = ad_plant.MapQDotToVelocity(ad_velocity_dynamics_context[context_index], (qn - q)/h)\n", " else:\n", " if not np.array_equal(q, plant.GetPositions(context[context_index])):\n", " plant.SetPositions(context[context_index], q)\n", " v_from_qdot = plant.MapQDotToVelocity(context[context_index], (qn - q)/h)\n", " return v - v_from_qdot\n", " for n in range(N-1):\n", " prog.AddConstraint(\n", " partial(velocity_dynamics_constraint, context_index=n),\n", " lb=[0]*nv, ub=[0]*nv,\n", " vars=np.concatenate(([h[n]], q[:,n], v[:,n], q[:,n+1])))\n", "\n", " # Contact forces\n", " contact_force = [prog.NewContinuousVariables(3, N-1, f\"foot{foot}_contact_force\") for foot in range(4)]\n", " for n in range(N-1):\n", " for foot in range(4):\n", " # Linear friction cone\n", " prog.AddLinearConstraint(\n", " contact_force[foot][0, n] <= mu * contact_force[foot][2, n])\n", " prog.AddLinearConstraint(\n", " -contact_force[foot][0, n] <= mu * contact_force[foot][2, n])\n", " prog.AddLinearConstraint(\n", " contact_force[foot][1, n] <= mu * contact_force[foot][2, n])\n", " prog.AddLinearConstraint(\n", " -contact_force[foot][1, n] <= mu * contact_force[foot][2, n])\n", " # normal force >=0, normal_force == 0 if not in_stance\n", " prog.AddBoundingBoxConstraint(\n", " 0, in_stance[foot, n] * 4 * 9.81 * total_mass,\n", " contact_force[foot][2, n])\n", "\n", " # Center of mass variables and constraints\n", " com = prog.NewContinuousVariables(3, N, \"com\")\n", " comdot = prog.NewContinuousVariables(3, N, \"comdot\")\n", " comddot = prog.NewContinuousVariables(3, N-1, \"comddot\")\n", " # Initial CoM x,y position == 0\n", " prog.AddBoundingBoxConstraint(0, 0, com[:2,0])\n", " # Initial CoM z vel == 0\n", " prog.AddBoundingBoxConstraint(0, 0, comdot[2,0])\n", " # CoM height\n", " prog.AddBoundingBoxConstraint(.125, np.inf, com[2,:])\n", " # CoM x velocity >= 0\n", " prog.AddBoundingBoxConstraint(0, np.inf, comdot[0,:])\n", " # CoM final x position\n", " if is_laterally_symmetric:\n", " prog.AddBoundingBoxConstraint(stride_length/2.0, stride_length/2.0, com[0,-1])\n", " else:\n", " prog.AddBoundingBoxConstraint(stride_length, stride_length, com[0,-1])\n", " # CoM dynamics\n", " for n in range(N-1):\n", " # Note: The original matlab implementation used backwards Euler (here and throughout),\n", " # which is a little more consistent with the LCP contact models.\n", " prog.AddConstraint(eq(com[:, n+1], com[:,n] + h[n]*comdot[:,n]))\n", " prog.AddConstraint(eq(comdot[:, n+1], comdot[:,n] + h[n]*comddot[:,n]))\n", " prog.AddConstraint(eq(total_mass*comddot[:,n], sum(contact_force[i][:,n] for i in range(4)) + total_mass*gravity))\n", "\n", " # Angular momentum (about the center of mass)\n", " H = prog.NewContinuousVariables(3, N, \"H\")\n", " Hdot = prog.NewContinuousVariables(3, N-1, \"Hdot\")\n", " prog.SetInitialGuess(H, np.zeros((3, N)))\n", " prog.SetInitialGuess(Hdot, np.zeros((3,N-1)))\n", " # Hdot = sum_i cross(p_FootiW-com, contact_force_i)\n", " def angular_momentum_constraint(vars, context_index):\n", " q, com, Hdot, contact_force = np.split(vars, [nq, nq+3, nq+6])\n", " contact_force = contact_force.reshape(3, 4, order='F')\n", " if isinstance(vars[0], AutoDiffXd):\n", " q = ExtractValue(q)\n", " if not np.array_equal(q, plant.GetPositions(\n", " context[context_index])):\n", " plant.SetPositions(context[context_index], q)\n", " torque = np.zeros(3)\n", " for i in range(4):\n", " p_WF = plant.CalcPointsPositions(context[context_index], foot_frame[i], [0,0,0], plant.world_frame())\n", " Jq_WF = plant.CalcJacobianTranslationalVelocity(\n", " context[context_index], JacobianWrtVariable.kQDot,\n", " foot_frame[i], [0, 0, 0], plant.world_frame(), plant.world_frame())\n", " ad_p_WF = InitializeAutoDiff(p_WF, np.hstack((Jq_WF, np.zeros((3, 18)))))\n", " torque = torque + np.cross(ad_p_WF.reshape(3) - com, contact_force[:,i])\n", " else:\n", " if not np.array_equal(q, plant.GetPositions(context[context_index])):\n", " plant.SetPositions(context[context_index], q)\n", " torque = np.zeros(3)\n", " for i in range(4):\n", " p_WF = plant.CalcPointsPositions(context[context_index], foot_frame[i], [0,0,0], plant.world_frame())\n", " torque += np.cross(p_WF.reshape(3) - com, contact_force[:,i])\n", " return Hdot - torque\n", " for n in range(N-1):\n", " prog.AddConstraint(eq(H[:,n+1], H[:,n] + h[n]*Hdot[:,n]))\n", " Fn = np.concatenate([contact_force[i][:,n] for i in range(4)])\n", " prog.AddConstraint(partial(angular_momentum_constraint, context_index=n), lb=np.zeros(3), ub=np.zeros(3),\n", " vars=np.concatenate((q[:,n], com[:,n], Hdot[:,n], Fn)))\n", "\n", " # com == CenterOfMass(q), H = SpatialMomentumInWorldAboutPoint(q, v, com)\n", " # Make a new autodiff context for this constraint (to maximize cache hits)\n", " com_constraint_context = [ad_plant.CreateDefaultContext() for i in range(N)]\n", " def com_constraint(vars, context_index):\n", " qv, com, H = np.split(vars, [nq+nv, nq+nv+3])\n", " if isinstance(vars[0], AutoDiffXd):\n", " if not autoDiffArrayEqual(qv, ad_plant.GetPositionsAndVelocities(com_constraint_context[context_index])):\n", " ad_plant.SetPositionsAndVelocities(com_constraint_context[context_index], qv)\n", " com_q = ad_plant.CalcCenterOfMassPositionInWorld(com_constraint_context[context_index])\n", " H_qv = ad_plant.CalcSpatialMomentumInWorldAboutPoint(com_constraint_context[context_index], com).rotational()\n", " else:\n", " if not np.array_equal(qv, plant.GetPositionsAndVelocities(context[context_index])):\n", " plant.SetPositionsAndVelocities(context[context_index], qv)\n", " com_q = plant.CalcCenterOfMassPositionInWorld(context[context_index])\n", " H_qv = plant.CalcSpatialMomentumInWorldAboutPoint(context[context_index], com).rotational()\n", " return np.concatenate((com_q - com, H_qv - H))\n", " for n in range(N):\n", " prog.AddConstraint(partial(com_constraint, context_index=n),\n", " lb=np.zeros(6), ub=np.zeros(6), vars=np.concatenate((q[:,n], v[:,n], com[:,n], H[:,n])))\n", "\n", " # TODO: Add collision constraints\n", "\n", " # Kinematic constraints\n", " def fixed_position_constraint(vars, context_index, frame):\n", " q, qn = np.split(vars, [nq])\n", " if not np.array_equal(q, plant.GetPositions(context[context_index])):\n", " plant.SetPositions(context[context_index], q)\n", " if not np.array_equal(qn, plant.GetPositions(context[context_index+1])):\n", " plant.SetPositions(context[context_index+1], qn)\n", " p_WF = plant.CalcPointsPositions(context[context_index], frame, [0,0,0], plant.world_frame())\n", " p_WF_n = plant.CalcPointsPositions(context[context_index+1], frame, [0,0,0], plant.world_frame())\n", " if isinstance(vars[0], AutoDiffXd):\n", " J_WF = plant.CalcJacobianTranslationalVelocity(context[context_index], JacobianWrtVariable.kQDot,\n", " frame, [0, 0, 0], plant.world_frame(), plant.world_frame())\n", " J_WF_n = plant.CalcJacobianTranslationalVelocity(context[context_index+1], JacobianWrtVariable.kQDot,\n", " frame, [0, 0, 0], plant.world_frame(), plant.world_frame())\n", " return InitializeAutoDiff(\n", " p_WF_n - p_WF, J_WF_n @ ExtractGradient(qn) - J_WF @ ExtractGradient(q))\n", " else:\n", " return p_WF_n - p_WF\n", " for i in range(4):\n", " for n in range(N):\n", " if in_stance[i, n]:\n", " # foot should be on the ground (world position z=0)\n", " prog.AddConstraint(PositionConstraint(\n", " plant, plant.world_frame(), [-np.inf,-np.inf,0], [np.inf,np.inf,0],\n", " foot_frame[i], [0,0,0], context[n]), q[:,n])\n", " if n > 0 and in_stance[i, n-1]:\n", " # feet should not move during stance.\n", " prog.AddConstraint(partial(fixed_position_constraint, context_index=n-1, frame=foot_frame[i]),\n", " lb=np.zeros(3), ub=np.zeros(3), vars=np.concatenate((q[:,n-1], q[:,n])))\n", " else:\n", " min_clearance = 0.01\n", " prog.AddConstraint(PositionConstraint(plant, plant.world_frame(), [-np.inf,-np.inf,min_clearance], [np.inf,np.inf,np.inf],foot_frame[i],[0,0,0],context[n]), q[:,n])\n", "\n", " # Periodicity constraints\n", " if is_laterally_symmetric:\n", " # Joints\n", " def AddAntiSymmetricPair(a, b):\n", " prog.AddLinearEqualityConstraint(a[0] == -b[-1])\n", " prog.AddLinearEqualityConstraint(a[-1] == -b[0])\n", " def AddSymmetricPair(a, b):\n", " prog.AddLinearEqualityConstraint(a[0] == b[-1])\n", " prog.AddLinearEqualityConstraint(a[-1] == b[0])\n", "\n", " AddAntiSymmetricPair(q_view.front_left_hip_roll,\n", " q_view.front_right_hip_roll)\n", " AddSymmetricPair(q_view.front_left_hip_pitch,\n", " q_view.front_right_hip_pitch)\n", " AddSymmetricPair(q_view.front_left_knee, q_view.front_right_knee)\n", " AddAntiSymmetricPair(q_view.back_left_hip_roll,\n", " q_view.back_right_hip_roll)\n", " AddSymmetricPair(q_view.back_left_hip_pitch,\n", " q_view.back_right_hip_pitch)\n", " AddSymmetricPair(q_view.back_left_knee, q_view.back_right_knee)\n", " prog.AddLinearEqualityConstraint(q_view.body_y[0] == -q_view.body_y[-1])\n", " prog.AddLinearEqualityConstraint(q_view.body_z[0] == q_view.body_z[-1])\n", " # Body orientation must be in the xz plane:\n", " prog.AddBoundingBoxConstraint(0, 0, q_view.body_qx[[0,-1]])\n", " prog.AddBoundingBoxConstraint(0, 0, q_view.body_qz[[0,-1]])\n", "\n", " # Floating base velocity\n", " prog.AddLinearEqualityConstraint(v_view.body_vx[0] == v_view.body_vx[-1])\n", " prog.AddLinearEqualityConstraint(v_view.body_vy[0] == -v_view.body_vy[-1])\n", " prog.AddLinearEqualityConstraint(v_view.body_vz[0] == v_view.body_vz[-1])\n", "\n", " # CoM velocity\n", " prog.AddLinearEqualityConstraint(comdot[0,0] == comdot[0,-1])\n", " prog.AddLinearEqualityConstraint(comdot[1,0] == -comdot[1,-1])\n", " prog.AddLinearEqualityConstraint(comdot[2,0] == comdot[2,-1])\n", "\n", " else:\n", " # Everything except body_x is periodic\n", " q_selector = PositionView([True]*nq)\n", " q_selector.body_x = False\n", " prog.AddLinearConstraint(eq(q[q_selector,0], q[q_selector,-1]))\n", " prog.AddLinearConstraint(eq(v[:,0], v[:,-1]))\n", "\n", " # TODO: Set solver parameters (mostly to make the worst case solve times less bad)\n", " snopt = SnoptSolver().solver_id()\n", " prog.SetSolverOption(snopt, 'Iterations Limits', 1e5 if running_as_notebook else 1)\n", " prog.SetSolverOption(snopt, 'Major Iterations Limit', 200 if running_as_notebook else 1)\n", " prog.SetSolverOption(snopt, 'Major Feasibility Tolerance', 5e-6)\n", " prog.SetSolverOption(snopt, 'Major Optimality Tolerance', 1e-4)\n", " prog.SetSolverOption(snopt, 'Superbasics limit', 2000)\n", " prog.SetSolverOption(snopt, 'Linesearch tolerance', 0.9)\n", " #prog.SetSolverOption(snopt, 'Print file', 'snopt.out')\n", "\n", " # TODO a few more costs/constraints from\n", " # from https://github.com/RobotLocomotion/LittleDog/blob/master/gaitOptimization.m\n", "\n", " result = Solve(prog)\n", " print(result.get_solver_id().name())\n", " #print(result.is_success()) # We expect this to be false if iterations are limited.\n", "\n", " def HalfStrideToFullStride(a):\n", " b = PositionView(np.copy(a))\n", "\n", " b.body_y = -a.body_y\n", " # Mirror quaternion so that roll=-roll, yaw=-yaw\n", " b.body_qx = -a.body_qx\n", " b.body_qz = -a.body_qz\n", "\n", " b.front_left_hip_roll = -a.front_right_hip_roll\n", " b.front_right_hip_roll = -a.front_left_hip_roll\n", " b.back_left_hip_roll = -a.back_right_hip_roll\n", " b.back_right_hip_roll = -a.back_left_hip_roll\n", "\n", " b.front_left_hip_pitch = a.front_right_hip_pitch\n", " b.front_right_hip_pitch = a.front_left_hip_pitch\n", " b.back_left_hip_pitch = a.back_right_hip_pitch\n", " b.back_right_hip_pitch = a.back_left_hip_pitch\n", "\n", " b.front_left_knee = a.front_right_knee\n", " b.front_right_knee = a.front_left_knee\n", " b.back_left_knee = a.back_right_knee\n", " b.back_right_knee = a.back_left_knee\n", "\n", " return b\n", "\n", "\n", " # Animate trajectory\n", " context = diagram.CreateDefaultContext()\n", " plant_context = plant.GetMyContextFromRoot(context)\n", " t_sol = np.cumsum(np.concatenate(([0],result.GetSolution(h))))\n", " q_sol = PiecewisePolynomial.FirstOrderHold(t_sol, result.GetSolution(q))\n", " visualizer.StartRecording()\n", " num_strides = 4\n", " t0 = t_sol[0]\n", " tf = t_sol[-1]\n", " T = tf*num_strides*(2.0 if is_laterally_symmetric else 1.0)\n", " for t in np.hstack((np.arange(t0, T, 1.0/32.0), T)):\n", " context.SetTime(t)\n", " stride = (t - t0) // (tf - t0)\n", " ts = (t - t0) % (tf - t0)\n", " qt = PositionView(q_sol.value(ts))\n", " if is_laterally_symmetric:\n", " if stride % 2 == 1:\n", " qt = HalfStrideToFullStride(qt)\n", " qt.body_x += stride_length/2.0\n", " stride = stride // 2\n", " qt.body_x += stride*stride_length\n", " plant.SetPositions(plant_context, qt[:])\n", " diagram.Publish(context)\n", "\n", " visualizer.StopRecording()\n", " visualizer.PublishRecording()\n", "\n", "# Try them all! The last two could use a little tuning.\n", "gait_optimization('walking_trot')\n", "#gait_optimization('running_trot')\n", "#gait_optimization('rotary_gallop')\n", "#gait_optimization('bound')" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "interpreter": { "hash": "aee8b7b246df8f9039afb4144a1f6fd8d2ca17a180786b69acc140d282b71a49" }, "kernelspec": { "display_name": "Python 3.6.9 64-bit", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.8.10" }, "metadata": { "interpreter": { "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6" } } }, "nbformat": 4, "nbformat_minor": 2 }