talos_plateformes_path.py 1 KB
Newer Older
1
from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
2
3


4
class PathPlanner(TalosPathPlanner):
5
6
7
8
9
    def set_rom_filters(self):
        super().set_rom_filters()
        #TEMP fix, because of issue https://github.com/humanoid-path-planner/hpp-fcl/issues/134 in hpp-fcl
        # we need to disable ROM checks in this scenario with really small contact surfaces
        self.rbprmBuilder.setFilter([])
10

11
    def run(self):
12
        self.root_translation_bounds = [-5, 5, -1.5, 1.5, 0.95, 1.3]
13
        self.init_problem()
14

15
16
        self.q_init[:3] = [0.18, 0.25, 1.14]
        self.q_goal[:3] = [1.07, 0.25, 1.14]
17

18
19
20
        self.init_viewer("multicontact/plateforme_surfaces",
                         reduce_sizes=[0.18, 0, 0],
                         visualize_affordances=["Support"])
21
22
23
        self.init_planner(kinodynamic=False)
        self.solve()
        self.display_path()
24
        # self.play_path()
25
        self.hide_rom()
26

27

28
29
30
if __name__ == "__main__":
    planner = PathPlanner()
    planner.run()