I'm simulating a differential drive robot with autonomous navigation in a warehouse.

Since I want that robot reaches also shelves positioned at a great height, I defined a prismatic joint along z axis in order to simulate an elevating platform.

I implemented the navigation stack with all the waypoints that the robot has to reach (using a cpp code), but I have no idea how to lift up the platform and then make my robot reach a specific height as waypoint.

Can someone help me?