Integration of laser-based localization

[simalpha]

@dljsjr: I am introducing a laser-based module to correct the drift in Valkyrie's state estimation. I am subscribing /ihmc_ros/valkyrie/output/robot_pose message generated by the IHMC state estimator, computing a correction using ICP-based point clouds registration and publishing a corrected pose.

It would be great if the state estimator could read this message and update the state. Would you be happy to re-introduce the interface for this in the IHMC code base? I refer to the topics /ihmc_ros/localization/pelvis_odom_pose_correction and /ihmc_ros/localization/pelvis_pose_correction which do not seem to be active anymore.

[dljsjr]

@simalpha I'll look in to re-introducing those topics in the next beta, but I give it a 60% chance that they won't work out of the box. It'll be easy enough to verify in sim but the robot in our lab is currently down due to an electrical issue.

We've also never tested that code on the Valkyrie hardware as we don't have our own external localizer (the ETH-based one we used to use is Atlas specific and we haven't had the chance to port it to Val), so even if the endpoint is still active I can't make any guarantees about the behavior of the robot when using them. Those endpoints work by "fusing" the external pose messages with the IHMC pose estimate so in certain situations (large translations, large orientation differences) things can behave strangely if the fusion is poorly tuned.

[mauricefallon]

Thanks Doug. We have a good understanding about what effect these corrections will have and are happy to do testing in sim to figure out what to expect with the robot.

We are really keen to get this functionality back and to contribute it back to you. (BTW, the ETH localizer is likely not to work out of the box because Valkyrie's head blocks most of the LIDAR FOV)

[dljsjr]

@mauricefallon That was my assumption as well, we just haven't bothered to test it.

I'll look in to having those topics reintroduced at some point today. I can do verification that the endpoints work in sim myself without an external localizer, but real-world behavior is going to be another story.

We really appreciate your willingness to help us test and debug this functionality, thanks!

[simalpha]

@dljsjr: I tested the localization in simulation and this is what I could achieve. A small modification to PelvisLinearStateUpdater.java causes the robot to drift forward and to the right when walking. When the robot stops walking, a corrected pose estimate is computed and published to /ihmc_ros/localization/pelvis_odom_pose_correction. The IHMC pose estimate gets updated with the correction.

However, is the message /ihmc_ros/localization/pelvis_odom_pose_correction needed or the other message without velocities (/ihmc_ros/localization/pelvis_pose_correction) can work? I am publishing a corrected pose to update position and orientation and I am not sure setting the velocities to zero is a good idea.

At the moment, if I publish to /ihmc_ros/localization/pelvis_pose_correction the pose estimate does not get updated with my correction. Do you know why?

[dljsjr]

@simalpha How large is the correction you're sending? Between some smoothing that goes on and the low real-time rate of the simulation sometimes there can be a significant amount of real world lag when trying to vet that endpoint in simulation.

I'll take a look at this as well, if it's truly not doing anything then we can open a new bug for this.

[simalpha]

Hi @dljsjr, I was trying to get a better understanding about how the controller deals with the topic /ihmc_ros/localization/pelvis_odom_pose_correction. When I publish a corrected pose to this topic (once every about 6s), I see the /ihmc_ros/valkyrie/output/robot_pose converging towards the corrected pose. However, it never converges to the exact pose. Given these veriables, set in class ClippedSpeedOffsetErrorInterpolator:

Z_DEADZONE_SIZE = 0.014;
Y_DEADZONE_SIZE = 0.014;
X_DEADZONE_SIZE = 0.014;
YAW_DEADZONE_IN_DEGREES = 1.0;

MAXIMUM_TRANSLATION_ERROR = 0.15;
MAXIMUM_ANGLE_ERROR_IN_DEGRESS = 10.0;  
MAX_TRANSLATIONAL_CORRECTION_SPEED = 0.05;
MAX_ROTATIONAL_CORRECTION_SPEED = 0.05;

Our understanding is:

• The pose correction has to be close to the current pose e.g. within 15cm or 10 degrees.
• The deadzone is a region centered in .../robot_pose which prevents the state to converge exactly to .../pelvis_odom_pose_correction. In this case, .../robot_pose moves towards the corrected pose if their distance is greater than 0.014 m and 1.0 degrees.

Is this correct? The second one is a safety procedure to stop us sending lots of high frequency corrections, right? Would it be safe to set

Z_DEADZONE_SIZE = 0.0;
Y_DEADZONE_SIZE = 0.0;
X_DEADZONE_SIZE = 0.0;
YAW_DEADZONE_IN_DEGREES = 0.0;

given that we generate a correction once every 6 seconds?

[dljsjr]

You are correct in your understanding.

The reason for the first safety is because stuff blows up when the correction is very large.

The reason for the second safety is because the localizer we used when this was developed updated at a much much faster rate than every 6 seconds, so if the robot was stationary and the corrections had converged then there would be noise about the true pose that would cause the robot to wiggle around.

I've also recently been informed that there's a good chance only the translational component of the pose corrector works, and that the orientation correction gets ignored. I'm investigating that a little more.

EDIT To answer your last question, feel free to play around w/ the deadzone parameters. We probably won't be able to accept a PR for them to be changed in code but if your localization strategy allows for no deadzone then we can look in to parameterizing these components with a topic or service or configuration file.

[simalpha]

@dljsjr Okay, thanks. I would be happy to get this to work as soon as possible.

I'm planning to set the dead-zone size to zero. I did it in simulation and it works just fine. I understand you think it should be safe to test it on the robot, right?

Cc: @mauricefallon

[dljsjr]

Well… I never used the word safe. I wholly support you trying to run it on the robot though. That's what estops are for.

[simalpha]

@dljsjr, @SylvainBertrand : I was trying to figure out what is the issue with the rotation correction not being properly interpreted. Actually, I have the impression it is not ignored, just the variable isRotationCorrectionEnabled is set to false by default. Once you enable it a correction to the rotation in _/ihmc_ros/localization/pelvis_odom_posecorrection affects the body pose but not as it should (could be that you expect to get a delta value for rotation - i.e. the correction only - and instead the corrected x-y-z values for translation?). The correction is interpreted in the wrong way but I cannot understand what is actually the logic you use for the update and the bug.

We wrote a script which publishes a corrected pose:

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from nav_msgs.msg import Odometry
import sys
import math
import numpy as np
#import lcm
#from bot_core.pose_t import pose_t
#lc = lcm.LCM()

pub = rospy.Publisher('/ihmc_ros/localization/pelvis_odom_pose_correction', Odometry, queue_size=10)

def quat_to_euler(q) :
  roll_a = 2.0 * (q[0]*q[1] + q[2]*q[3]);
  roll_b = 1.0 - 2.0 * (q[1]*q[1] + q[2]*q[2]);
  roll = math.atan2 (roll_a, roll_b);

  pitch_sin = 2.0 * (q[0]*q[2] - q[3]*q[1]);
  pitch = math.asin (pitch_sin);

  yaw_a = 2.0 * (q[0]*q[3] + q[1]*q[2]);
  yaw_b = 1.0 - 2.0 * (q[2]*q[2] + q[3]*q[3]);  
  yaw = math.atan2 (yaw_a, yaw_b);
  return [roll,pitch,yaw]

def euler_to_quat(rpy):
  roll =  rpy[0]
  pitch = rpy[1]
  yaw =   rpy[2]

  sy = math.sin(yaw*0.5);
  cy = math.cos(yaw*0.5);
  sp = math.sin(pitch*0.5);
  cp = math.cos(pitch*0.5);
  sr = math.sin(roll*0.5);
  cr = math.cos(roll*0.5);
  w = cr*cp*cy + sr*sp*sy;
  x = sr*cp*cy - cr*sp*sy;
  y = cr*sp*cy + sr*cp*sy;
  z = cr*cp*sy - sr*sp*cy;
  return np.array([w,x,y,z])

def callback(m):
    rospy.loginfo(rospy.get_caller_id() + "I heard %f", m.pose.pose.position.x)

    orientation = [m.pose.pose.orientation.w, m.pose.pose.orientation.x, m.pose.pose.orientation.y, m.pose.pose.orientation.z]
    rpy = quat_to_euler(orientation)
    rpy[2] = rpy[2] + 10.0*math.pi/180.0
    orientation = euler_to_quat(rpy)  
    m.pose.pose.orientation.w = orientation[0]
    m.pose.pose.orientation.x = orientation[1]
    m.pose.pose.orientation.y = orientation[2]
    m.pose.pose.orientation.z = orientation[3]

    m.pose.pose.position.x = m.pose.pose.position.x + 0.15
    pub.publish(m)

    #msg = pose_t()
    #msg.utime = m.header.stamp.secs*1E6 + m.header.stamp.nsecs*1E-3
    #msg.pos = [m.pose.pose.position.x, m.pose.pose.position.y, m.pose.pose.position.z]
    #msg.orientation = 
    #lc.publish('POSE_BODY_CORRECTION_ROS', msg.encode() )

    print "Sent pose correction, exiting"
    rospy.signal_shutdown("Exiting")

def listener():
    rospy.init_node('send_fake_correction_ros', anonymous=True)
    rospy.Subscriber("/ihmc_ros/valkyrie/output/robot_pose", Odometry, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    listener()

Could you please help me debugging this? We have a deadline in few days time and need to get this to work.

Cc: @mauricefallon

[dljsjr]

I believe that the orientation correction flag is set to false by default because we never got it to work correctly. It's why we disabled the feature.

The way the orientation correction is supposed to work is that you provide a corrected yaw (not a delta) and the corrector runs an alpha filter to smoothly transition towards that yaw. However, this doesn't work.

The longer short version, which is probably not a very satisfying answer, is that the localization stuff is sorta dead code. It was implemented experimentally during the DRC by a visiting researcher, and it had enough bugs that we were forced to triage it for the sake of the competition by asking if the state estimation performance for Atlas was good enough without it for us to focus on other stuff. It was only ever used on Atlas where the drift in yaw was negligible compared to the drift in position so we just… never fixed it.

Since this issue is closed I'm going to open a new issue for getting the yaw correction portion of the pelvis pose corrector to work.