Skip to content

Point Robot Envs

cbfpy.envs.point_robot_envs

Simulation environments for point robots

PointRobotEnv

Bases: BaseEnv

Simulation environment for a point robot trying to approach a target position in 3D, while remaining in a safe set defined as a box

Parameters:

Name Type Description Default
xyz_min ArrayLike

Minimum bounds of the safe region, shape (3,). Defaults to (-1.0, -1.0, -1.0)

 (-1.0, -1.0, -1.0)
xyz_max ArrayLike

Maximum bounds of the safe region, shape (3,). Defaults to (1.0, 1.0, 1.0)

 (1.0, 1.0, 1.0)
Source code in cbfpy/envs/point_robot_envs.py 
17
18
19
20
21
22
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
85
86
87
88
89
class PointRobotEnv(BaseEnv):
    """Simulation environment for a point robot trying to approach a target position in 3D,
    while remaining in a safe set defined as a box

    Args:
        xyz_min (ArrayLike, optional): Minimum bounds of the safe region, shape (3,). Defaults to (-1.0, -1.0, -1.0)
        xyz_max (ArrayLike, optional): Maximum bounds of the safe region, shape (3,). Defaults to (1.0, 1.0, 1.0)
    """

    # Constants
    # Based on the values in the point robot URDF
    URDF = "point_robot.urdf"
    RADIUS = 0.25
    MASS = 1.0

    def __init__(
        self,
        xyz_min: ArrayLike = (-1.0, -1.0, -1.0),
        xyz_max: ArrayLike = (1.0, 1.0, 1.0),
    ):
        self.xyz_min = np.array(xyz_min)
        self.xyz_max = np.array(xyz_max)
        assert len(self.xyz_min) == len(self.xyz_max) == 3
        with stdout_redirected(restore=False):
            self.client: pybullet = BulletClient(pybullet.GUI)
        self.client.setAdditionalSearchPath(find_assets_dir())
        self.client.configureDebugVisualizer(self.client.COV_ENABLE_GUI, 0)
        self.robot = self.client.loadURDF(self.URDF)
        self.target = self.client.loadURDF(self.URDF, basePosition=(3, 1, 1))
        self.client.changeVisualShape(self.target, -1, rgbaColor=[1, 0, 0, 1])
        self.client.changeDynamics(self.target, -1, linearDamping=10, angularDamping=10)
        self.box = visualize_3D_box(
            [self.xyz_min - self.RADIUS, self.xyz_max + self.RADIUS],
            rgba=(0, 1, 0, 0.5),
        )

        # For color determination
        self.is_in_box = True
        self.tol = 1e-3

    def _update_color(self, robot_pos: ArrayLike) -> None:
        """Update the color of the box depending on if the robot is inside or not (Green inside, red outside)"""
        robot_pos = np.array(robot_pos)
        if np.any(robot_pos < self.xyz_min - self.tol) or np.any(
            robot_pos > self.xyz_max + self.tol
        ):
            if self.is_in_box:
                self.client.changeVisualShape(self.box, -1, rgbaColor=[1, 0, 0, 0.5])
            self.is_in_box = False
        else:
            if not self.is_in_box:
                self.client.changeVisualShape(self.box, -1, rgbaColor=[0, 1, 0, 0.5])
            self.is_in_box = True

    def get_state(self) -> Array:
        robot_pos = self.client.getBasePositionAndOrientation(self.robot)[0]
        robot_vel = self.client.getBaseVelocity(self.robot)[0]
        self._update_color(robot_pos)
        return np.array([*robot_pos, *robot_vel])

    def get_desired_state(self) -> Array:
        target_pos = self.client.getBasePositionAndOrientation(self.target)[0]
        target_vel = self.client.getBaseVelocity(self.target)[0]
        return np.array([*target_pos, *target_vel])

    def apply_control(self, u: Array) -> None:
        robot_pos = self.client.getBasePositionAndOrientation(self.robot)[0]
        self.client.applyExternalForce(
            self.robot, -1, u, robot_pos, self.client.WORLD_FRAME
        )

    def step(self):
        self.client.stepSimulation()

PointRobotObstacleEnv

Bases: BaseEnv

Simulation environment for a point robot avoiding a movable obstacle, while remaining in a safe set defined as a box

Parameters:

Name Type Description Default
robot_pos ArrayLike

Initial position of the robot. Defaults to (0, 0, 0).

 (0, 0, 0)
robot_vel ArrayLike

Initial velocity of the robot. Defaults to (0, 0, 0).

 (0, 0, 0)
obstacle_pos ArrayLike

Initial position of the obstacle. Defaults to (1, 1, 1).

 (1, 1, 1)
obstacle_vel ArrayLike

Initial velocity of the obstacle. Defaults to (-0.5, -0.6, -0.7).

 (-0.5, -0.6, -0.7)
xyz_min ArrayLike

Minimum bounds of the safe region, shape (3,). Defaults to (-1.0, -1.0, -1.0)

 (-1.0, -1.0, -1.0)
xyz_max ArrayLike

Maximum bounds of the safe region, shape (3,). Defaults to (1.0, 1.0, 1.0)

 (1.0, 1.0, 1.0)
Source code in cbfpy/envs/point_robot_envs.py 
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
class PointRobotObstacleEnv(BaseEnv):
    """Simulation environment for a point robot avoiding a movable obstacle,
    while remaining in a safe set defined as a box

    Args:
        robot_pos (ArrayLike, optional): Initial position of the robot. Defaults to (0, 0, 0).
        robot_vel (ArrayLike, optional): Initial velocity of the robot. Defaults to (0, 0, 0).
        obstacle_pos (ArrayLike, optional): Initial position of the obstacle. Defaults to (1, 1, 1).
        obstacle_vel (ArrayLike, optional): Initial velocity of the obstacle. Defaults to (-0.5, -0.6, -0.7).
        xyz_min (ArrayLike, optional): Minimum bounds of the safe region, shape (3,). Defaults to (-1.0, -1.0, -1.0)
        xyz_max (ArrayLike, optional): Maximum bounds of the safe region, shape (3,). Defaults to (1.0, 1.0, 1.0)
    """

    # Constants
    # Based on the values in the point robot URDF
    URDF = "point_robot.urdf"
    RADIUS = 0.25
    MASS = 1.0

    def __init__(
        self,
        robot_pos: ArrayLike = (0, 0, 0),
        robot_vel: ArrayLike = (0, 0, 0),
        obstacle_pos: ArrayLike = (1, 1, 1),
        obstacle_vel: ArrayLike = (-0.5, -0.6, -0.7),
        xyz_min: ArrayLike = (-1.0, -1.0, -1.0),
        xyz_max: ArrayLike = (1.0, 1.0, 1.0),
    ):
        with stdout_redirected(restore=False):
            self.client: pybullet = BulletClient(pybullet.GUI)
        self.client.setAdditionalSearchPath(find_assets_dir())
        self.client.configureDebugVisualizer(self.client.COV_ENABLE_GUI, 0)
        self.robot = self.client.loadURDF(self.URDF, basePosition=robot_pos)
        self.target = self.client.loadURDF(self.URDF, basePosition=obstacle_pos)
        self.client.changeVisualShape(self.target, -1, rgbaColor=[1, 0, 0, 1])
        self.client.changeDynamics(self.target, -1, angularDamping=10)
        self.client.resetBaseVelocity(self.target, obstacle_vel, (0, 0, 0))
        self.client.resetBaseVelocity(self.robot, robot_vel, (0, 0, 0))
        self.client.addUserDebugPoints([[0, 0, 0]], [[1, 0, 0]], 10, 0)
        self.xyz_min = np.array(xyz_min)
        self.xyz_max = np.array(xyz_max)
        self.box = visualize_3D_box(
            [self.xyz_min - self.RADIUS, self.xyz_max + self.RADIUS],
            rgba=(0, 1, 0, 0.5),
        )

    def get_state(self):
        robot_pos = self.client.getBasePositionAndOrientation(self.robot)[0]
        robot_vel = self.client.getBaseVelocity(self.robot)[0]
        obstacle_pos = self.client.getBasePositionAndOrientation(self.target)[0]
        obstacle_vel = self.client.getBaseVelocity(self.target)[0]
        return np.array([*robot_pos, *robot_vel]), np.array(
            [*obstacle_pos, *obstacle_vel]
        )

    def get_desired_state(self):
        return np.zeros(6)

    def apply_control(self, u):
        robot_pos = self.client.getBasePositionAndOrientation(self.robot)[0]
        self.client.applyExternalForce(
            self.robot, -1, u, robot_pos, self.client.WORLD_FRAME
        )

    def step(self):
        self.client.stepSimulation()