[core] Add Perlin ground profile generators.

[mwulfman]

I am proposing to add a unidirection Perlin ground HeightmapFunction, 2D Perlin ground HeightmapFunction and a quantization methods to quantize any ground to certain z-values.

• Unidirectional Perlin Ground:

• 2D Perlin Ground

• Pass the Perlin process by copy to the builder function
• Ground quantization method to quantized the ground to only certain level
• Unit tests
• Fix sample_state in gym-jiminy to account for the ground profile ?

[duburcqa]

It would be nice to add a quantization options to quantized the ground to only certain level.

What do you mean ?

[mwulfman]

It would be nice to add a quantization options to quantized the ground to only certain level.

What do you mean ?

To be able to get this type of terrain:
(ref: https://fse.studenttheses.ub.rug.nl/30171/1/thesis.pdf)

[duburcqa]

I see. It looks like a quantisation based on the vertical value only. Something like z = std::floor(z / delta) * delta. The issue is that we still need to add small slopes...

I think tiles or tile2dInterp1d should be extended to support "tile-ization" of any ground profile. The advantage is that it does not require having access to the normal of the original ground profile !

[duburcqa]

Are you planning to work on it ? If not, then I suggest closing this issue because I'm not planning to work on it either.

[mwulfman]

Yes, I did a bit more work on it but didn’t push anything yet. I’m planning to work on/finish it in a couple of weeks.

[duburcqa]

Ok perfect ! Let's keep it open then :)

[duburcqa]

Apparently you made good progress ! Nice :)

What is your future plan ? You want to add quantisation ? Can you add a unit test checking if the gradient is right ?

[duburcqa]

leads to floating point exception (core dumped)

The problem is not that it asserts to false because asserts are disabled for release build type. The issue is rather the contrary, we should systematically check this condition and raise an exception if not satisfying this condition causes segfault.

I will issue a fix for this.

[mwulfman]

Apparently you made good progress ! Nice :)

What is your future plan ? You want to add quantisation ? Can you add a unit test checking if the gradient is right ?

Yes, I still need to do :

• some update of the code (following your comments as well),
• add quantisation on any ground,
• since I'm already into Perlin noise, I'm thinking of adding 2d perlin noise/ground
• Yes, add unit test for the gradient. I was thinking of comparing the gradient with the numerical gradient, were you thinking of any other tests?
• Maybe update sample_state in gym-jiminy to account for the HeightMap.

[duburcqa]

I was thinking of comparing the gradient with the numerical gradient, were you thinking of any other tests?

Nope it is fine :)

Maybe update sample_state in gym-jiminy to account for the HeightMap.

I'm afraid doing this is not straightforward. It has been a long-lasting issue as you know. Once everything else is working, I could help you on this part to improve the current heuristic that has not sufficient in many cases.

[duburcqa]

To many code duplication. There should be no need to implement anything specific for the 2D scenario.

[duburcqa]

I'm going to help you on this, so that we can close this long-lasting PR.

[mwulfman]

I'm going to help you on this, so that we can close this long-lasting PR.

If you have time for it, I'm pretty happy to get your help. I didn't have much time to work on it the last few weeks, but hoping to finish it this week.

[duburcqa]

Ok I skimmed through the code. Now I remember. Since those processes at not used for anything inside core itself, I'm going to refactor them a bit. In particular to replace operator() by evaluate and add evaluate_gradient which will return both the value and its gradient. Some internal methods also need a better name, such as grad that is confusing now. Then, I will rather template them with the dimensionality of the process, with only 1 and 2 being supported at the moment. I hope this will make everything more simple and clear, while avoid redundancy.

[mwulfman]

Since you're taking over for the PerlinNoise part, I started creating some unit tests (using the current methods' name) and fixed a typo in the 1D gradient computation. There is also a bug in the 2D gradient computation.

[duburcqa]

Ok nice, so far I have been working on adding the gradient feature on all the other random processes so as to provide a unified API for all of them, without slowing down computations. I have also fixed stuff like negative time, and fixed bugs on non-perlin processes.

[duburcqa]

I will commit something this morning.

[mwulfman]

Can you try creating a periodic_perlin_ground on your side ? I am having a bit of trouble doing so:

In [15]: periodic_perlin_ground?
Docstring: periodic_perlin_ground( (float)wavelength, (float)period, (int)num_octaves, (int)seed) -> HeightmapFunction
Type:      function

In [16]: periodic_perlin_ground(1.0,2.0,6,0)
---------------------------------------------------------------------------
LogicError                                Traceback (most recent call last)
Cell In[16], line 1
----> 1 periodic_perlin_ground(1.0,2.0,6,0)

LogicError: jiminy::PeriodicPerlinNoiseOctave<2>::PeriodicPerlinNoiseOctave(/Users/mathiasw/Code/jiminy/core/include/jiminy/core/utilities/random.hxx:457):
'period' must be larger than 'wavelength'.

[mwulfman]

Also, I had started working on sample_state and added:

gym_jiminy/common/envs/locomotion.py, line 239..

        # Compute the height of the freeflyer in neutral configuration
        # TODO: Take into account the ground profile.
        q_init, _ = self._sample_state()

        ground_profile = self.simulator.get_options()["world"]["groundProfile"]
        self._height_neutral = q_init[2] - ground_profile(q_init[:2])[0]

gym_jiminy/common/envs/generic.py, line 1398

        # Get ground profile
       ground_profile = self.simulator.get_options()["world"]["groundProfile"]

       # Lowest contact point from ground
       contact_frames_xy_positions = np.stack([self.robot.pinocchio_model.frames[frame_index].placement.translation[:2]
                                   for frame_index in self.robot.contact_frame_indices])
       contact_frames_z_positions = np.stack([self.robot.pinocchio_model.frames[frame_index].placement.translation[2]
                                   for frame_index in self.robot.contact_frame_indices])
       ground_height_contact_frames = np.zeros((len(self.robot.contact_frame_indices),))
       query_heightmap(ground_profile, contact_frames_xy_positions, ground_height_contact_frames)
       delta_height_from_ground = contact_frames_z_positions - ground_height_contact_frames

       if (delta_height_from_ground >= 0.0).all():
           # Lowest=Closest contact points needs to be into contact
           # with the ground:
           q[2] -= min(delta_height_from_ground)
       else:
           # From the points below ground, deepest one needs to be in contact
           # with the ground
           delta_height = delta_height_from_ground[delta_height_from_ground < 0.0]
           q[2] = q[2] - delta_height.min()

Either there is a bug in this code or there is something that I've been missing because it's not working as expected.

[duburcqa]

Either there is a bug in this code or there is something that I've been missing because it's not working as expected.

No need to make two branches based on the sign of delta_height_from_ground. This should be fine:

        # Get ground profile
       ground_profile = self.simulator.get_options()["world"]["groundProfile"]

       # Lowest contact point from ground
       contact_frames_xy_positions = np.stack([
           self.robot.pinocchio_model.frames[frame_index].placement.translation[:2]
           for frame_index in self.robot.contact_frame_indices])
       contact_frames_z_positions = np.stack([
           self.robot.pinocchio_model.frames[frame_index].placement.translation[2]
           for frame_index in self.robot.contact_frame_indices])
       ground_height_contact_frames = np.zeros((len(self.robot.contact_frame_indices),))
       query_heightmap(ground_profile, contact_frames_xy_positions, ground_height_contact_frames)
       delta_height_from_ground = contact_frames_z_positions - ground_height_contact_frames

       # Make sure that there is exactly one point in contact with the ground, 
       # all the others being above the ground
       q[2] -= min(delta_height_from_ground)

Apart from that, it looks good. What is the issue ?

[mwulfman]

The issue is that it is flying when I test it on Perlin ground...

Also the API changed on query_heighmap so it needs to be:

        contact_frames_xy_positions = np.stack([self.robot.pinocchio_model.frames[frame_index].placement.translation[:2]
                                    for frame_index in self.robot.contact_frame_indices], -1)

[duburcqa]

Can you try creating a periodic_perlin_ground on your side ? I am having a bit of trouble doing so:

It is expected, but the error message is misleading. I will fix it. In fact, each octave has a larger wavelength than the "previous" one, so if you add too many octaves, at some point it can exceed the period.

[mwulfman]

Can you try creating a periodic_perlin_ground on your side ? I am having a bit of trouble doing so:

It is expected, but the error message is misleading. I will fix it. In fact, each octave has a larger wavelength than the "previous" one, so if you add too many octaves, at some point it can exceed the period.

Is it standard implementation or some implementation use the other convention (each octave has a higher frequency) ?

[duburcqa]

The issue is that it is flying when I test it on Perlin ground...

Did you updated the forward kinematic by calling update_quantities or framesForwardKinematics ?

# Set robot in neutral configuration
q = self._neutral()
pin.framesForwardKinematics(self.robot.pinocchio_model, self.robot.pinocchio_data, q)

[duburcqa]

Is it standard implementation or some implementation use the other convention (each octave has a higher frequency) ?

It is standard, look for Perlin Noise "persistence" and "lacunarity".

[mwulfman]

The issue is that it is flying when I test it on Perlin ground...

Did you updated the forward kinematic by calling update_quantities or framesForwardKinematics ?

# Set robot in neutral configuration
q = self._neutral()
pin.framesForwardKinematics(self.robot.pinocchio_model, self.robot.pinocchio_data, q)

It shouldn't be needed since it is already called after sample_state in reset:

        # Sample the initial state and reset the low-level engine
        q_init, v_init = self._sample_state()
        if not jiminy.is_position_valid(self.robot.pinocchio_model, q_init):
            raise RuntimeError(
                "The initial state provided by `_sample_state` is "
                "inconsistent with the dimension or types of joints of the "
                "model.")

        # Set robot in initial configuration
        pin.framesForwardKinematics(
            self.robot.pinocchio_model, self.robot.pinocchio_data, q_init)

[duburcqa]

Also the API changed on query_heighmap so it needs to be:

Yes, it changes for leveraging vectorisation more efficiently and hence improve performance.

[duburcqa]

It shouldn't be needed since it is already called after sample_state in reset:

Indeed. If you called refresh afterward then there is something very strange happening.

[mwulfman]

Here is the code snippet I use. I simply call env.render() at breakpoint :

import numpy as np

from gym_jiminy.envs.anymal import ANYmalPDControlJiminyEnv
from jiminy_py import tree
from jiminy_py.core import periodic_perlin_ground, random_perlin_ground


if __name__ == '__main__':
    env = ANYmalPDControlJiminyEnv()
    engine_options = env.simulator.get_options()
    orientation = 0.0
    wavelength= 1.0
    num_octaves = 8
    seed = 42
    engine_options["world"]["groundProfile"] =  random_perlin_ground(wavelength, num_octaves, seed)
    env.simulator.set_options(engine_options)
    for i in range(5):
        obs, _ = env.reset(seed = i)
        terminated, truncated = False, False
        while not (terminated or truncated):
            assert not any(np.isnan(e).any() for e in tree.flatten(obs)), "Nan in observation"

            obs, _, terminated, truncated, _ = env.step(0.0*env.action_space.sample())
            breakpoint()
            if terminated or truncated:
                print(f"terminated: {terminated}, truncated: {truncated}")
        env.replay(backed="panda3d")
        env.stop()

[duburcqa]

Here is your issue:

            contact_positions = np.stack([
                self.robot.pinocchio_data.oMf[frame_index].translation
                for frame_index in self.robot.contact_frame_indices], -1)

self.robot.pinocchio_model.frames[frame_index].placement is the relative pose of the frame wrt to its parent (it is constant).

[duburcqa]

I can confirm it is working with:

            engine_options = self.simulator.get_options()
            ground_fun = engine_options['world']['groundProfile']

            # Lowest contact point from ground
            contact_positions = np.stack([
                self.robot.pinocchio_data.oMf[frame_index].translation
                for frame_index in self.robot.contact_frame_indices], -1)
            contact_heights = np.zeros((contact_positions.shape[-1],))
            jiminy.query_heightmap(
                ground_fun, contact_positions[:2], contact_heights)
            delta_height_from_ground = contact_positions[2] - contact_heights

            # Make sure that there is exactly one point in contact with the
            # ground, all the others being above the ground.
            q[2] -= delta_height_from_ground.min()

[mwulfman]

Oh yes, that's a nice catch. It is still floating in the air on my end..

[duburcqa]

It is fine on my side ! (I mean with "your" patch to make the robot touch the ground, it is not merged for now as it is not generic enough, it should also consider collision bodies, even if they are not properly supported at the time being that are used in ant environment for instance). Ideally, compute_freeflyer_state_from_fixed_body should be fixed to what it is supposed to do, but doing it properly it is quite difficult.

[duburcqa]

I need to profile this code, because I'm not happy at all with the performance:

import numpy as np
from jiminy_py.core import random_perlin_ground, query_heightmap

ground_fun = random_perlin_ground(wavelength=1.0, num_octaves=8, seed=42)
positions = np.random.rand(3, 100)

%timeit query_heightmap(ground_fun, positions[:2], positions[2])
76.1 µs ± 680 ns per loop (mean ± std. dev. of 7 runs, 10,000 loops each)

I consider this as extremely bad. I would expect something like 5us at max. There is a lot of redundant computations everywhere: the gradient is computed independently from the height, and the gradient at knots is never cached even if the query point is only shifted by 1mm.

[duburcqa]

Damn... most of the time is spend it computing sincos and pow... sounds tricky to optimise...