Valkyrie neck control problem

scpeters commented 7 years ago

While testing valkyrie with the SRC software (Ubuntu Trusty 14.04, ROS Indigo, ihmc-open-robotics-software c787635deb and the latest versions of ihmc ros code (0.9.x)), I've reproduced an issue with Valkyrie's neck control that was first reported on our bitbucket issue tracker:

https://bitbucket.org/osrf/srcsim/issues/96/head-trajectory-pitch-047-hangs-head-neck

Steps to reproduce issue

Send a head pitch command of 0.5 radians. The head will tilt forward but then fall forward much farther than 0.5 rad and be unable to lift the head back up. Here is a video and rosbag file made by using our python script for keyboard teleop:

Expected behavior

I would expect the head to stay near 0.5 radians and be able to lift itself back up when I reset the command to 0 radians.

Actual behavior

The head pitches forward and is unable to lift itself back up. I did some additional testing, and 10 Nm is enough to lift the head back up, and the lower neck has an effort limit of 26 Nm and a Kp gain of 50, but the control command seems to saturate. I've made another video (without a matching rosbag, sorry) that has plots of the pitch angle and torque:

https://vimeo.com/207209488

Contributors with relevant knowledge or expertise (optional)

I don't know if this is a problem in the NASA code or IHMC, but I'll cc @dljsjr

dljsjr commented 7 years ago

As far as I know there's nothing in our software that would saturate the control loop and the gains were tuned on the real Valkyrie hardware. I'll get with @SylvainBertrand on this from our end to see if we can confirm or deny that it's in our controller before pinging the JSC team.

dljsjr commented 7 years ago

@scpeters just to confirm: some people on your tracker indicate that joint angle control works but head orientation control doesn't in this edge case? Is that accurate?

dljsjr commented 7 years ago

@scpeters Based on the information on the bug on your tracker, I think I know what's happening, but I have a few questions.

The underlying cause is not control becoming saturated or poorly tuned gains/lack of effort commands being emitted, it's because the redundant pitch joints in the neck kinematics creates a very large singularity when trying to compute joint angles from a desired head orientation. So once the head is in a certain orientation w.r.t the rest of the robot the orientation commands become unsolvable and the head orientation manager stops doing anything at all.

The questions I have are:

Rud Merriam made a comment about the "controller" collapsing. Does this mean the entire robot collapses or just the neck?
You mentioned that you are able to apply a torque directly to the neck to get out of the singularity. Is it also possible to use neck trajectory messages to recover from the singularity?

I talked to @SylvainBertrand and we're not confident there's really a fix to this issue because of the nature of it. We could put together hacks or kludges to do singularity avoidance or singularity escape but a.) It wouldn't be deterministic or predictable without introducing a ton of visualization and feedback, and b.) it would still be possible to use neck joint commands to put the head in to a singularity that would not be escapable using orientation commands.

I think our options are to either remove that endpoint entirely and only allow for joint angle commands, or to just heavily document that message and let users know of the workaround. But before we make any decisions I'd like some feedback on those two questions I asked above.

scpeters commented 7 years ago

The neck joint commands don't usually get into that trouble, but I've also not sure they can recover from the problem. I'll see if I can reproduce the problem with neck joint control.

scpeters commented 7 years ago

The keyboard teleop script has conservative joint limits for neck joint control, so it doesn't get into the problem on its own. If I give it a disturbance, however, it will tip the neck over and not be able to get up. I wonder if gravity compensation is backwards on the neck joint? Maybe the ihmc model has diverged from val_description? I'm uploading a video right now that has plots of both upper and lower neck pitch joints.

scpeters commented 7 years ago

Here's the video I promised:

https://vimeo.com/207351516

scpeters commented 7 years ago

First the upper neck joint is commanded to -0.1 rad and then the lower neck joint is commanded to 0.5 rad. It works properly, but with a little nudge, the head falls over and gets stuck.

scpeters commented 7 years ago

Ok, it seems like the gains for the lower neck joint were a little low on the JSC side. This raises the Ki gain, which fixed the problem for me:

https://gitlab.com/nasa-jsc-robotics/val_description/merge_requests/17

dljsjr commented 7 years ago

Awesome. The singularity issue is still a real problem and we can revisit it if it comes up for the teams. Since this is an improvement on the JSC side I'm going to close.

scpeters commented 7 years ago

This can be manually patched by modifying the following code block in /opt/nasa/indigo/share/val_description/model/urdf/valkyrie_sim_gazebo_sync.urdf:

  <joint name="lowerNeckPitch" type="revolute">
    <origin rpy="0.0 0.0 0.0" xyz="0.0203518 0.0 0.33845"/>
    <axis xyz="0 1 0"/>
    <parent link="torso"/>
    <child link="lowerNeckPitchLink"/>
    <limit effort="26" lower="0" upper="1.162" velocity="5"/>
    <controller_gains Kd="3" Ki="3" Kp="50"/>
    <dynamics damping="0.1" friction="0.0"/>

make sure that Ki="3"

scpeters commented 7 years ago

Actually, that patch won't work without an additional change to val_gazebo. The following should work though:

  <joint name="lowerNeckPitch" type="revolute">
    <origin rpy="0.0 0.0 0.0" xyz="0.0203518 0.0 0.33845"/>
    <axis xyz="0 1 0"/>
    <parent link="torso"/>
    <child link="lowerNeckPitchLink"/>
    <limit effort="26" lower="0" upper="1.162" velocity="5"/>
    <controller_gains Kd="0" Ki="0" Kp="150"/>
    <dynamics damping="10" friction="0.0"/>

ihmcrobotics / ihmc_valkyrie_ros