Obtaining Height Data/ Elevation Maps/ RGB-D sensing

[waleedbkhalid]

Hi,

I am making this post to request any information/ guidance regarding the mentioned subject. This would help me with my ongoing research project.

So, for my project, I need to obtain the elevation maps/ height data for a legged robot. I am listing down the features and options I have explored so far.

1. Using the RGB-D camera on anymalC robot. The anymalC robot is equipped with a realsensed435 camera. Since this is an RGB-D camera, it should be able to give the robot the functionality to sense data. What I have gauged with the given pieces of code and information on the web is that a C++ function for using this sensor is not available. Python needs to make a call to this function eventually to sense the RGB-D data. Correct me if I am wrong and such a function is available. However, the documentation also says that this part is currently under development.

2. Using the getHeight() function for heightMap objects. Now this function uses (x,y) coordinate to tell height a point. While I think I may be able to come up with a piece of code that defines a domain of y and x and obtains the height for those points, I am concerned about how a robot would be able to get this height data eventually. It is obviously possible to build my algorithm on top of the height data coming this way, but eventually to connect all the dots, I want the robot to sense the height.

3. I explored Husky robot in the rayDemo2 example. It does use a lidar. So my first question is, given the piece of code below from the rayDemos2 file:

create visual boxes to visualize scan points

for i in range(scanSize1):
for j in range(scanSize2):
scans.append(server.addVisualBox("box" + str(i) + "/" + str(j), 0.3, 0.3, 0.3, 1, 0, 0))

for i in range(10000000000):
server.integrateWorldThreadSafe()
lidarPos = robot.getFramePosition("imu_joint")
lidarOri = robot.getFrameOrientation("imu_joint")

for i in range(scanSize1):
    for j in range(scanSize2):
        yaw = j * math.pi / scanSize2 * 0.6 - 0.3 * math.pi
        pitch = -(i * 0.3/scanSize1) + 0.2
        normInv = 1. / math.sqrt(pitch * pitch + 1)
        direction = np.mat([np.cos(yaw) * normInv, np.sin(yaw) * normInv, -pitch * normInv], dtype=np.float64)
        rayDirection = lidarOri.dot(direction.transpose())
        col = world.rayTest(lidarPos, rayDirection, 30)
        if col.size() > 0:
            scans[i * scanSize2 + j].setPosition(col.at(0).getPosition())
        else:
            scans[i * scanSize2 + j].setPosition(np.array([0, 0, 100]))

would I be able to manipulate the scan variable and get some elevation or height info,

if Yes, then is it possible that this lidar can be added to the anymalC or anymalB robot?

Any advice on this matter would be deeply appreciated by me. I would be very grateful for all the valuable insight and I look forward to hearing your thoughts soon.

[jhwangbo]

1. please check out sensors.cpp example. These sensors are available through raisimUnreal
2. I don't understand your question.
3. As you see above in your code, lidar can be added to any robot. In fact, you don't even need a robot. If you know (x,y), you can get the height info. I am not clear about this part of your question

[waleedbkhalid]

So for 2 I was just reconfirming that height can be obtained from x,y coordinates, Correct?

Like you mentioned in 3, that (x,y) can be used to obtain height data. Is this the getHeight() method of the heightmap object that you are talking about?

If I add the lidar, can you elaborate on what information the scan variable stores. Is it depth info?

[jhwangbo]

Yes, you can get the height using getHeight(). That is the only method to access the height information.

If you want to create a general lidar, you have to use the rayTest(). You can access the position as you wrote above

col = world.rayTest(lidarPos, rayDirection, 30)
        if col.size() > 0:
            scans[i * scanSize2 + j].setPosition(col.at(0).getPosition())
        else:
            scans[i * scanSize2 + j].setPosition(np.array([0, 0, 100]))

Here, col is a collision object and you can get many other information like which object was in contact, which part of the body and so on

[waleedbkhalid]

Hi,

So I have been successful in using the lidar for the wheeled robot. But when I do it for the anymalC, I tried to attach it to the IMU joint which did not work. Then I used the lidar cage joint, and extremely small readings came (I speculate that only the lidar cage was being sensed).

What would be a good position on the anymalC robot to add the lidar sensor.

[jhwangbo]

Simply remove the collision body. Otherwise, you need an accurate cage collision body.