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


4
class PathPlanner(TalosPathPlanner):
5
6
7
    def load_rbprm(self):
        from talos_rbprm.talos_abstract import Robot
        # select ROM model with really conservative ROM shapes
8
9
        Robot.urdfName = "talos_trunk_large_reducedROM"
        self.robot_node_name = "talos_trunk_large_reducedROM"
10
11
12
        Robot.rLegId = 'talos_rleg_rom_reduced'
        Robot.lLegId = 'talos_lleg_rom_reduced'
        Robot.urdfNameRom = [Robot.lLegId, Robot.rLegId]
13
14
        self.used_limbs = Robot.urdfNameRom
        self.rbprmBuilder = Robot()
15

16
        # As the ROM names have changed, we need to set this values again (otherwise it's automatically done)
17
18
        self.rbprmBuilder.setReferenceEndEffector(Robot.lLegId, self.rbprmBuilder.ref_EE_lLeg)
        self.rbprmBuilder.setReferenceEndEffector(Robot.rLegId, self.rbprmBuilder.ref_EE_rLeg)
19
20
21
22
23

    def init_problem(self):
        super().init_problem()
        # greatly increase the number of loops of the random shortcut
        self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
24
25
        self.ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True)
        self.ps.setParameter("Kinodynamic/verticalAccelerationBound", 3.)
26
27
28

    def compute_extra_config_bounds(self):
        # bounds for the extradof : by default use v_max/a_max on x and y axis and a large value on z axis
29
30
31
32
        self.extra_dof_bounds = [
            -self.v_max, self.v_max, -self.v_max, self.v_max, -10, 10, -self.a_max, self.a_max, -self.a_max,
            self.a_max, -10, 10
        ]
33
34

    def run(self):
35
        self.root_translation_bounds = [-1.5, 3, 0., 3.3, 0.95, 2.]
36
37
        self.init_problem()

38
39
40
        self.q_init[:2] = [0., 1.2]
        self.q_goal[:2] = [1.87, 1.2]
        self.q_goal[2] += 0.6
41

42
        self.init_viewer("multicontact/bauzil_stairs", reduce_sizes=[0.08, 0., 0.], visualize_affordances=["Support"])
43
44
45
        self.init_planner()
        self.solve()
        self.display_path()
46
        # self.play_path()
47
48
        self.hide_rom()

49

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