Using the instantaneousQPController for a position controlled robot

[FlorianReimold]

Hi,

I am trying to use Drake as Balancing and Walking Controller for the position controlled Thor Mang robot. I started with the Atlas Examples and modified those to get them working with Thor.

Now I face the problem, that the instantaneousQPController only provides torque, accelerations and reference velocity as output. To get a fully body pose I tried to simply multiply the reference velocity with the timestep which got me reasonable results for the arms, but fails in situations of contact, in particular for the legs. Did I overlook a possibility to get a whole body pose, or does anybody have any advice for me on how to approach that problem?

[RussTedrake]

Sounds great.

@kuindersma and @rdeits should weigh in on this, too. We did almost exactly this on the arms for Atlas (integrating the desired accelerations), but did have force control on the legs. It worked well enough for us.

What exactly is your failure mode in the contact situation? Do you feel like your integration needs to better respect the kinematic constraints? You could imagine putting that in (even with something like an LCP).

[FlorianReimold]

My problem was, that the robot would not keep it's feet on the ground because it tried to maintain the given velocities in the leg joints. The result was Thor jumping off the ground (just simulation of course). However, I discovered that the velocityReference function uses an internal integrator to compute v_ref. By just disabling that (setting eta = 1.0), I am getting much better results. I was really surprised how well the robot maintains the position of the feet in simulation. At the moment, Thor still tilts over backwards when balancing with drake. I think tuning of parameters should somehow prevent that.

[rdeits]

Hm, that's interesting. We also found eta to be a useful number to tune for Atlas for certain situations, although our control mode was a bit different.

Specifically, we found that while standing or walking, the vref integrator on the ankle joints could build up enough to cause the feet to slowly roll until they were not longer making good contact with the ground. Setting eta to a nonzero value (around 0.001 or 0.005) seemed to help prevent that. During more aggressive maneuvers like push recovery, I also found it useful to completely drop the integral terms on the ankle joints when in contact, which led to much better compliance against poorly modeled terrain. Of course, we were dealing with a force- and velocity-controlled robot, so our range of good eta values was a lot different.

[FlorianReimold]

I think my problem with eta was, that I only use v_ref to compute the position. When I start the controller, all velocities start at 0 and it needs time for them to sum up to reasonable values.

My Thor can now balance with Drake without tilting. The simulator introduced quite a bit of an error in the hip joints. The reference velocity computed by Drake was not able to compensate that. I am now using very high gains for pelvis and joints and multiplying each velocity command by 10. I am not sure if this is the way to go, but it works for the moment. If one of you guys have a better idea please let me know about it! I also tried to walk, but this still makes Thor falling backwards, probably because of the same reason.

[FlorianReimold]

Hi, I am still struggling with Drake and Thor. While everything works fine in the force-controlled drake simulation, Thor refuses to speed up it's pelvis while walking in Gazebo (position controlled). Currently Drake is only getting the pelvis pose as input (I use the desired position as feedback, not the actual position, because drake would constantly increase the tracking error). I made a short video showing the problem: https://www.youtube.com/watch?v=aVuAz2atVgw The left foot is the starting foot and one can see Thor trying to place it's weight on the right foot, but when Drake decides to raise the left foot, the pelvis hasn't reached it's destination causing Thor to tilt. Are there any parameters that could help me to fix that? I also think that I might not have understood the high level "intention" of drake for the pelvis motion. What does the controller actually try to do with it?

[RussTedrake]

would be interested to hear what @kuindersma thinks about this one.

[RussTedrake]

@FlorianReimold -- out of curiousity, have you made any progress with this?

[FlorianReimold]

No, I did not made any further progress. I decided to move on and try a different approach (I need to get something working since this is my master thesis!). Both the simulated Thor and the real hardware is now walking quite well with Missura's Capturestep Framework. The instantaneousQPController from your Atlas-Walk will still appear in my thesis. Can you point me to some kind of publication dedicated to that controller?

[RussTedrake]

Will look forward to reading what you come up with. We have a number of relevant papers on my publication page, perhaps the DRC controls summary in Autonomous Robots would be the best citation. Thx

[kuindersma]

Sorry, I just figured out that I have to change my github email notification settings.

For the arm links in Atlas, we didn't double integrate to get positions references, we used PID to compute desired arm joint accelerations and made those hard constraints in the QP.

It's not easy to guess what the problem is from your video/description, but from the look of it I'd make sure the transformation of floating base velocities is correct. Drake uses Euler-dot by default and Gazebo uses angular velocity.