Constraint drift in a complex linkage

[rdeits]

So I was feeling a bit inspired by #478 and decided to take a brief break from thesis-ing :wink:

https://www.youtube.com/watch?v=rvhsje29Alg&feature=youtu.be

All the relevant code is here: https://github.com/rdeits/StrandbeestRobot.jl/blob/master/Strandbeest%20simulation.ipynb , including generating the non-tree joints by parsing the loop_joint tags in the URDF and solving for a consistent initial configuration using Optim.jl.

Overall it was pretty painless: the nonlinear solve with Optim seems to work fine, and once I got the cost function right I didn't have any issues with local minima. It takes a couple of seconds to find the initial configuration on my laptop--I'm sure we could do better, but this is pretty good.

Simulation works fine, but only up to a point. The constraints still exponentially drift apart, and the mechanism basically falls apart (and starts returning NaN states) after a little more than 4 seconds.

But still, this is pretty cool and I think it would be great to have this as a fully working demo.

[tkoolen]

Awesome! I'll take look soonish.

[rdeits]

Potentially relevant: most of the constraints seem to be perfectly stable over the course of the simulation. In fact, for every leg, there is exactly one joint that comes uncoupled during simulation, and it's always the loop joint between bars b/d/e (which form a solid unit) and bar c: https://files.tested.com/photos/2015/04/05/74549-leg-measurement.jpg . In the URDF, those are the joints with names pairXX_legX_loop_b_c. Notably, those are the only parts of the mechanism where a single link (bar_c) is the successor to two different loop joints (loop_b_c and loop_a_c).

[tkoolen]

That's very useful info, thanks.

[tkoolen]

Where is the URDF?

[tkoolen]

Ah, I guess just https://raw.githubusercontent.com/RobotLocomotion/drake/last_sha_with_original_matlab/drake/examples/Strandbeest/Strandbeest.urdf.

[rdeits]

It's in the same repo as that notebook: https://github.com/rdeits/StrandbeestRobot.jl/tree/master/data

[tkoolen]

Why is link 'k' a child of link 'j' in the tree? I would have expected it to be a child of link 'm'.

[rdeits]

I don't know. Probably an artifact of my attempt to keep as much of the complexity as possible contained within the single-leg .xacro file. Links a, l, and m have to be shared between a pair of legs, so all the joints involving them have to be defined in https://github.com/rdeits/StrandbeestRobot.jl/blob/master/data/LegPair.xacro instead of https://github.com/rdeits/StrandbeestRobot.jl/blob/master/data/LegAssembly.xacro . It should be perfectly possible to change that arrangement, though.

[tkoolen]

Seems to be a problem with the proportional term. Setting the P gain to zero makes this work pretty well.

In particular, the angular portion of the proportional term.

Nope, it was actually a problem with the linear part of the proportional term.

By the way, the following loop joint error cost function should work for all loop joint types:

"""
Return a function which maps a configuration vector
for the given mechanism to the total squared error in
the alignment of the robot's non-tree joints.
"""
function loop_joint_error(robot)
    let statecache = StateCache(robot), resultcache = DynamicsResultCache(robot)
        function(q)
            state = statecache[eltype(q)]
            result = resultcache[eltype(q)]
            set_configuration!(state, q)
            zero_velocity!(state)
            gains = rbd.CustomCollections.ConstDict{JointID}(SE3PDGains(PDGains(1, 0), PDGains(1, 0)))
            rbd.constraint_bias!(result, state; stabilization_gains=gains)
            ret = result.constraintbias ⋅ result.constraintbias
        end
    end
end

Also, using RigidBodySim this simulates at about 6.5x realtime on my machine. The visualizer callback has trouble keeping up though.

[rdeits]

Wow, that's astonishing. I'll update the notebook file