Adds a intermediate step when rising for more stability

[16stelter]

Proposed changes

Related issues

Necessary checks

• [ ] Update package version
• [ ] Run catkin build
• [ ] Write documentation
• [ ] Create issues for future work
• [ ] Test on your machine
• [ ] Test on the robot
• [ ] Put the PR on our Project board

[jaagut]

@16stelter what's the status of this PR? Is this still needed?

[jaagut]

Similar to #409