talos_flatGround_path.py 1.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
10
11
12
13
14

    def init_problem(self):
        self.a_max = 0.05
        super().init_problem()
        # greatly increase the number of loops of the random shortcut
        self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 50)
        # force the base orientation to follow the direction of motion along the Z axis
        self.ps.setParameter("Kinodynamic/forceYawOrientation", True)


15
16
    def run(self):
        self.init_problem()
17

18
        self.q_init[:2] = [0, 0]
19
20
21
22
23
        self.q_goal[:2] = [1., 0]

        # Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
        #self.q_init[-6:-3] = [0.1, 0, 0]
        #self.q_goal[-6:-3] = [0, -0.1, 0]
24

25
        self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
26
        self.init_planner(True, False)
27
28
        self.solve()
        self.display_path()
29
        # self.play_path()
30
        self.hide_rom()
31

32

33
if __name__ == "__main__":
34
    planner = PathPlanner()
35
    planner.run()