Clean up formulation of Differential IK

[RussTedrake]

The tested use case has been more "open-loop" where the current robot state is integrated forward from previous commands.

Per discussion with @siyuanfeng-tri , the interface taking a desired pose is a little odd -- it's effectively implementing a proportional control with the gains hidden in the cost function + whatever velocity => position integration that the user does after the fact. There might be a more natural PD on pose formulation that could be implemented as a separate/different wrapper on the core DiffIK call.

Will revisit this soon.

[RussTedrake]

Confirmed that switching to "open-loop" mode resolved the undamped unconstrained motion. But in the process, I documented the existing mathematical program and thought a bit about what improvements I would make to it. Some of them have been implemented already (but not sufficiently tested) in https://github.com/RussTedrake/drake/tree/diff_ik_update

Recommendation 1: The current interface adds cost to all DOF which could interfere with the optimization of alpha. I think a better formulation is

 *   min_{v_next,alpha}   | alpha - |V| |^2
 *                        + (v_next - v_desired)'(I - J'J'⁺)(v_next - v_desired)
 *                        // with v_desired = k_p*(q_nominal - q_current)
 *                        // and J'⁺ the pseudo-inverse of J'.
 *
 *   s.t. J*v_next = alpha * V / |V|  // J*v_next has the same direction as V
 *        joint_lim_min <= q_current + v_next*dt <= joint_lim_max
 *        joint_vel_lim_min <= v_next <= joint_vel_lim_max
 *        joint_accel_lim_min <= (v_next - v_current)/dt <=
 *          joint_accel_lim_max.

where I've also removed the unconstrained joint limits which I believe were only added in as a band-aid?

Recommendation 2: differential IK should NOT have any notion of timestep. As it is, it's already confusing (is it the timestep of the controller? the plant?). I think that acceleration limits could be easily recast as Δv constraints (and the caller can scale by Δt if they so choose). The joint limits would be cleaner thinking of them as they would be implemented in continuous time -- they get added only if the joint limit is active (so on the current q, instead on q_next).

Not sure if I have time to finish/clean this right now, so I hope this leaves sufficient bread-crumbs.

[RussTedrake]

I've finally taken some time to think through the diff IK formulations we are using. My notes (relatively complete, but still in need of spit and polish) are here: http://manipulation.csail.mit.edu/pick.html#section10

I'd like to get consensus on the formulation, and then (if necessary) move forward with changing drake's implementation. @siyuanfeng-tri , @hongkai-dai -- might you be willing to read those notes, and compare them with the current implementation described here?

In my view, there are a few things to change about the current drake implementation: 1) the objective has an unnecessary (and I would think undesirable) scaling by |V|. And I think we can add 0 <= \alpha <= 1, which means we should really make that a linear objective instead of quadratic. That could make 6 dof robots solve faster. 2) the joint-space centering has two quirks, I think.
a) the desired goal isn't projected into the nullspace, so it can interfere with the primary objective even when it should not. because we have Jv = alpha V as a constraint, presumably this could only slow down our tracking, not introduce tracking error, but I would still like to fix it. b) we have an unconstrained joint velocity limit that has always felt strange (an l-infinty norm in some unknown coordinates that are whatever we were given by SVD), and an objective asking q_next ~= q_nominal. That's too much to ask... and I suspect we are active on that constraint most of the time (bang-bang). I think it would be better to ask for a proportional control law in the objective.

After we agree if we should change, and the formulation, then I'd like to think about the strategy for making the change. It should include hardware testing if possible.

[siyuanfeng-tri]

I think both suggestions are great. I hacked the thing together one night and moved on without a second look (typical..) The unconstrained join velocity was a total hack in response to commanding to q_norm in one tick. Not sure why I didn't do a P control. Should definitely fix that... I like how you made everything sounds so accessible in the text book :) Do you intend to add the collision avoidance bits in the diff ik controller as well? I think that would fit nicely here.

[hongkai-dai]

which means we should really make that a linear objective instead of quadratic.

BTW, if we want to solve an LP, then we need to investigate a good LP solver. Currently we don't have a good open-source LP solver in drake. OSQP sometimes don't solve LP nicely (it is designed for QP). We have SCS and CSDP, but neither of them is a nice LP solver.

[hongkai-dai]

I think the manipulation notes look great. A couple of minor thoughts

• In section 3.6.1, first paragraph, it should be ℝ⁶ˣ⁷, namely it should be \times instead the letter x between 6 and 7.
• Below equation 20, " but satisfy my joint limit constraints" should be "but satisfy my velocity limit constraints"? I think joint limit constraints can regarded as the joint angle limits.
• In equation 23, I am not entirely sure that we should upper bound α by 1, which says we can run slower than the v^G, but not faster. What is the rationale to say the end effector commanded velocity should not exceed v^G?

[RussTedrake]

@hongkai-dai -- Re: typos... thanks.
Re: alpha <= 1. This seems sensible to me from an applications standpoint (don't want the robot moving faster than we command). But in the case where the other constraints we add are joint, velocity, and acceleration limits (all including zero as a feasible solution), I also think that one could show that having those constraints active would never cause the solution to speed up.
Re: LP solvers. Is that a reason to avoid changing the cost function? I could certainly write (\alpha - 1)^2, but that seems silly? In every example we use, there is a nullspace, so it will still be a QP.

[RussTedrake]

@siyuanfeng-tri -- thanks. i haven't talked about collision avoidance yet. will definitely do that when we get to motion planning. (I also ended up pulling IK out of this chapter... it's really turning into an exercise on stripping out the non-essentials). But it's a really good point, so I have added a breadcrumb in the text source to remind myself to link forward to that part of the solution once those notes exist.

[siyuanfeng-tri]

I second alpha <= 1 for the same reason. Don't run faster than what was commanded. For the cost part, I might have done |V| to avoid LP. Maybe not fully expanding the collision avoidance part, but I think it's worth mentioning that we can formulate it as linear constraints.

[hongkai-dai]

I see, it makes sense to constraint alpha <= 1.

For LP part, sorry I don't mean we should change to the quadratic cost. I think the LP formulation is great. Just want to say currently we don't have a good open source LP solver in Drake, and I should add one.

[EricCousineau-TRI]

For reformulation, is it worth mentioning / accommodating for "sticking" / "sliding" cases, at least for collision avoidance as a constraint of dist_min(q) + ddist_dq * v * K >= security_distance *?

"sticking" - if you constrain direction, then commanding a end-effector velocity that may "graze" the collision constraint will cause you to come to halt (vs. sliding). Ideal when really needing grasp precision, and you want other things to avoid collision (e.g. the elbow colliding w/ environment).

"sliding" - if you can relax direction (but still make LP / QP feasible and aligned with desired velocity), then you can get sliding. Ideal when your desired spatial velocities (or poses) may be a bit "sloppy" (e.g. teleop, etc.).

---

* As @avalenzu originally found in the setup in Faverjon & Tournassoud's "velocity damper", set K = (influence_distance - security_distance) / xi (see Eq. (6) and Appendix II in their paper, https://hal.inria.fr/inria-00075933/PDF/RR-0621.pdf). Alternatively, you can set K = dt.

[RussTedrake]

@EricCousineau-TRI the formulation in the notes is definitely the "sticking" case. If you want the "sliding" case, then I think you want a fundamentally different formulation (without the velocity direction constraint); not to monkey-patch this one.

I would be fine offering multiple formulations within the same function (toggled by a parameter), but I think it would be a different mathematical program.

[RussTedrake]

I believe the existing test described in https://github.com/RobotLocomotion/drake/blob/90d8b0d4c8eaaace6e44279db18ce2cf8c0396b9/manipulation/planner/test/differential_inverse_kinematics_test.cc#L146-L152 is incorrect. Adding the constraint that v[0] == v[1] to the iiwa should not be a "positive" test -- diff IK should not be able to find a solution where the commanded velocity is in the same direction as the desired velocity. The (basis of the) nullspace of the Jacobian in this configuration is P = -0.599053 -0.761689 2.15106 6.68645e-16 -1.99565 1 0.158224. v[0] == v[1] is not in the nullspace of J.

My new implementation correctly, I believe, considers the system to be "stuck" giving this command and this constraint. I just want to comment / document it here to call attention to it before I change the test.