pumpkin_2d_generator.py 4.2 KB
Newer Older
Johannes Bier's avatar
Johannes Bier committed
1
2
3
4
5
import jinja2
import numpy as np
import os
import rospkg

6
7
8

AVAILABLE_TYPES = ["pumpkin_01", "pumpkin_02", "pumpkin_03", "cylinder", "maize_01", "maize_02", "beans_01"]

Johannes Bier's avatar
Johannes Bier committed
9
10
11
12
13
14
15
16
17
18
19
20
class Pumpkin2DGenerator():
    def __init__(self,
        pumpkin_radius = .15,
        row_width = .7,
        pumpkin_offset = .60,
        max_angle_variation = np.deg2rad(5),
        num_pumpkin_pairs = 5, 
        num_rows_left = 2,
        num_rows_right = 3,
        pumpkin_height = 0.3,
        pumpkin_mass = 5.0,
        radius_noise_range = 0.05,
21
22
        seed = None,
        types=",".join(AVAILABLE_TYPES)):
Johannes Bier's avatar
Johannes Bier committed
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
        
        for k,v in locals().items():
            self.__setattr__(k,v)
        np.random.seed(self.seed)
        self.reset()
    
    def reset(self):
        position = np.matrix([[0],[0],[1]])
        self.way_points = [position]
        self.world_T_local = np.identity(3)
        angles = []
        self.pumpkins = []
        
        for _ in range(self.num_pumpkin_pairs):
            angle_variation = ((np.random.rand() * 2) - 1) * self.max_angle_variation
            angles.append(angle_variation)

            tx = self.pumpkin_offset
            ty = 0

            T = np.matrix([
                [1, 0, tx],
                [0, 1, ty],
                [0, 0, 1]
            ])

            R = np.matrix([
                [ np.cos(angle_variation), np.sin(angle_variation), 0],
                [-np.sin(angle_variation), np.cos(angle_variation), 0],
                [                       0,                       0, 1]
            ])

            self.world_T_local = self.world_T_local * T * R
            position = self.world_T_local * np.matrix([[0],[0],[1]])
            self.way_points.append(position)
            
            for i in range(self.num_rows_left):
                width = self.row_width * (i + 0.5)
                pumpkin_1 = self.world_T_local * np.matrix([[0],[ width],[1]])
                self.pumpkins.append(pumpkin_1)

            for i in range(self.num_rows_right):
                width = self.row_width * (i + 0.5)
                pumpkin_2 = self.world_T_local * np.matrix([[0],[-width],[1]])
                self.pumpkins.append(pumpkin_2)

        self.pumpkins = np.array(np.stack([p.T for p in self.pumpkins])).T
        
    def get_distance_to_closest_pumpkin(self, x, y, theta):
        s = np.sin(theta)
        c = np.cos(theta)
        LOCAL_T_WORLD = np.matrix([
            [c, -s, (-x*np.cos(2*theta) - x + y*np.sin(2*theta))/(2*c)],
            [s,  c,                                         -x*s - y*c],
            [0,  0,                                                  1]])

        LOCAL_pumpkins = LOCAL_T_WORLD * self.pumpkins
        dist_pumpkin = np.linalg.norm(LOCAL_pumpkins[0:2], axis=0).min() - self.pumpkin_radius
        return dist_pumpkin
    
    def render_to_template(self, template):
        def into_dict(xy, radius, height, mass, index):
85
86
87
88
89
90
            types = []
            for t in self.types.split(","):
                if t in AVAILABLE_TYPES:
                    types.append(t)
                else:
                    print("Error: model type", t, "is not available.")
Johannes Bier's avatar
Johannes Bier committed
91
92
93
94
95
96
97
98
99
100
101
102
103
104
            coordinate = dict()
            coordinate["type"] = np.random.choice(types)
            inertia = dict()
            inertia["ixx"] = (mass * (3 * radius**2 + height**2)) / 12.
            inertia["iyy"] = (mass * (3 * radius**2 + height**2)) / 12.
            inertia["izz"] = (mass * radius**2) / 2.
            coordinate["inertia"] = inertia
            coordinate["mass"] = mass
            coordinate["x"] = xy[0]
            coordinate["y"] = xy[1]
            coordinate["radius"] = radius + (2*np.random.rand() - 1)*self.radius_noise_range
            if coordinate["type"] == "cylinder":
                coordinate["height"] = height
            coordinate["name"] = "{}_{:04d}".format(coordinate["type"], index)
105
            coordinate["yaw"] = np.random.rand() * 2.0 * np.pi
Johannes Bier's avatar
Johannes Bier committed
106
107
108
            return coordinate
        coordinates = [into_dict(row,self.pumpkin_radius, self.pumpkin_height, self.pumpkin_mass, i) for i, row in enumerate(self.pumpkins.T[:,:-1])]
        template = jinja2.Template(template)
109
        return template.render(coordinates=coordinates, package_path=rospkg.RosPack().get_path('kamaro_world'))