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


4
class PathPlanner(TalosPathPlanner):
5
6
    def load_rbprm(self):
        from talos_rbprm.talos_abstract import Robot
7
8
        Robot.urdfName = "talos_trunk_large"  # load the model with conservative bounding boxes for trunk
        self.robot_node_name = "talos_trunk_large"
9
10
11
12
13
14
15
16
17
18
19
        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 run(self):
        self.init_problem()
20
        self.root_translation_bounds = [-1.5, 4, 0., 3.3, self.rbprmBuilder.ref_height, self.rbprmBuilder.ref_height]
21
22
        self.set_joints_bounds()

23
        self.q_init[:2] = [-0.9, 1.7]
24
25
        # Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
        self.q_init[-6:-3] = [0.07, 0, 0]
26
        self.q_goal[:2] = [3.6, 1.2]
27
28
29
30
31
32
        self.q_goal[-6:-3] = [0, -0.1, 0]

        self.init_viewer("multicontact/floor_bauzil", visualize_affordances=["Support"])
        self.init_planner()
        self.solve()
        self.display_path()
33
        # self.play_path()
34
35
        self.hide_rom()

36

37
38
39
if __name__ == "__main__":
    planner = PathPlanner()
    planner.run()