Fix/dynup ik init

[16stelter]

Proposed changes

Init was causing weird ik solutions when motors were off previously. Now it uses the same approach as the walking and sets critical motors to a safe pose first.

Related issues

Necessary checks

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