hjsuh94 / irs_mpc Goto Github PK

Broadly, we can consider three separate axis of how we should be modeling contact:

1. Which order of dynamics should we consider?

• Quasistatic Formulation (throw away the mass matrix)
• Quasidynamic Formulation (throw away the momentum at next timestep)
• Full second-order Formulation (consistent with Newton's law)

2. How should we handle contact?

• Penalty Method (application of stiff spring / dampers)
• Event-driven integration of hybrid dynamics
• Implicit time-stepping with LCP (Stewar t& Trinkle) / QP (made convex with Anitescu)

3. What should the contact geometry be? (Closely related to how we should detect / quantify contact)

• Point to point contact (sphere)
• Point to surface contact / Surface to surface contact (Box / polytope geometries)
• Hydroelastic contact

For position controlled systems, it could be the case that ubound depends on a nominal point on x_trj, but the current code structure only allows uniform bounds.

Examples to try:

• Plate-pickup (Reasonable success)
• Hongkai and Albert's three-finger example
• Carrot problem (Failure)

Analysis to try: compare quality of gradients for some popular contact examples.

• Compare for simple collision case with damping.
• Compare for grazing example with friction.

For TV-LQR it'd be nice to provide an option of doing transverse linearization, as opposed to naive time-varying.

How do we make a fair comparison between trajopt using quasistatic and second-order simulation? It might be worth our time to address this question before we spend engineering time into writing the second order sim.

From my brief effort from trying to write a second-order MBP sim, we need to think about the following big topics:

1. Differences between Quasidynamic and Second order

1. The number of variables:

• The quasidynamic simulations only operate on position variables q, while the second-order simulation needs to take into account velocities q and v. This goes beyond the differences in how the dynamics are formulated - the specification into the optimal control problem is already different.

2. The number of timesteps

• In the quasistatic sim, the timestep of the simulator (let's call this dt) is the timestep of the collocation point for the trajopt algorithm (let's call this h). However, this cannot be true for second-order sim as typically, dt=1e-3, and this would result in too much variables needed for collocation.
• If we truly say dt has to be equal to h (such as in Michael's paper), the ability to take big timesteps during simulation is simply a huge win. The second-order sim simply cannot beat the variable-efficiency of the quasidynamic method.
• If dt is allowed to be different from h, this also leaves the question of the interleaving period (i.e. how different can it be?) What's the loss of using h=0.1 and dt=1e-3 for the second-order sim?

3. The input variable

• The input to the second-order model are forces and torques, while the input to the quasidynamic model is the position command.
• This can be somewhat mitigated by hooking up a position PD controller in the outer-level loop of the dynamics.
• Recall that iLQR starts from a feasible guess, usually in the form of an input trajectory. If the input trajectory comes from force, there is no way to provide a good initial force trajectory as a guess (As opposed to SQP, where we can also provide x)

2. The Performance Metrics

1. The quality of the gradient
2. The quality of the trajectory

Summary of Meeting

1. If we want to implement better threading (without multiple processes), Drake's MonteCarlo code that Calder implemented should be informative.
2. We need to compare irs_lqr on quasistatic model vs. second-order model, in order to decouple the effectiveness of irs_lqr and the effectiveness of doing trajopt on a quasistatic model. We believe that the main advantages of doing a quasistatic model are two-fold:

• Quasistatic sim allows taking longer timesteps stably. (Alejandro's Todorov relaxation might be an interesting contender to this)
• Quasistatic sim allows more stable gradients by defining dynamics over longer horizons.
  Seems like we can roughly come up with the following table:

3. Hongkai's work on hydroelastic vs. point contact might be relevant here. Would be nice to talk to Hongkai.
4. Gradient vs. KKT condition. Is the gradient actually large (i.e. we're up against true non-differentiability) or are we just computing it the bad way?

• If the solver has a set precision, this might interfere with the noise of the inversion (?)

5. Sphere geometry helps with decoupling hydroelastic with point contact, since the instabilities of. There are basically three sources of discontinuities:

• Discontinuity of collision
• Discontinuity of slip / stick (friction)
• Discontinuity of shapes (signed distance function).
  Hydroelastic, in principle, should be trying to address the third issue in the same spirit.

6. It seems like it's okay that we optimize locally, but it would be super important to make it more concrete (the more non-convex the dynamics become, the less effective local methods become, and methods like CEM / MPPI become more successful)

TODO is to make a clear interface for how the variance scheduling function (eta in the paper draft) enters into the system.

Each example should own their yaml file, which is shared by the file that actually runs the main irs thread (e.g. run_planar_hand.py), the workers planar_hand_workers.py, and the setup planar_hand_setup.py.

As a rule of thumb, we do not want to edit anything in irs_lqr between different examples and iterations.

Sort out list of important hyperparameters here:

• first-order vs. zero-order vs. exact