talos_navBauzil_path.py 2.04 KB
Newer Older
1
from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
2
import numpy as np
3
4


5
6
7
8
9
10
class PathPlanner(TalosPathPlanner):

    status_filename = "/res/infos.log"

    def load_rbprm(self):
        from talos_rbprm.talos_abstract import Robot
11
12
        Robot.urdfName = "talos_trunk_large"  # load the model with conservative bounding boxes for trunk
        self.robot_node_name = "talos_trunk_large"
13
14
15
16
17
18
19
20
21
22
23
24
25
        self.rbprmBuilder = Robot()

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

    def set_random_configs(self):
        """
        randomly sample initial and goal configuration :
        """
26
        from hpp.corbaserver.rbprm.tools.sample_root_config import generate_random_conf_without_orientation
27
28
29
30
31
        self.q_init = generate_random_conf_without_orientation(self.rbprmBuilder, self.root_translation_bounds)
        self.q_goal = generate_random_conf_without_orientation(self.rbprmBuilder, self.root_translation_bounds)
        print("q_init= " + str(self.q_init))
        print("q_goal= " + str(self.q_goal))
        # write problem in files :
32
33
34
        with open(self.status_filename, "w") as f:
            f.write("q_init= " + str(self.q_init) + "\n")
            f.write("q_goal= " + str(self.q_goal) + "\n")
35
36
37

    def run(self):
        self.init_problem()
38
        self.root_translation_bounds = [-1.5, 3, 0., 3.3, self.rbprmBuilder.ref_height, self.rbprmBuilder.ref_height]
39
40
41
42
43
44
        self.set_joints_bounds()
        self.init_viewer("multicontact/floor_bauzil", visualize_affordances=["Support"])
        self.set_random_configs()
        self.init_planner()
        self.solve()
        self.display_path()
45
        # self.play_path()
46
47
        self.hide_rom()

48

49
50
51
if __name__ == "__main__":
    planner = PathPlanner()
    planner.run()