Driving Backwards
Index
Starting to go backwards
The main problem with this phase is that the simulator is designed to drive exclusively forwards, in fact, the reward system is based on the time and distance you drive forwards.
DirectController() class has been used for the movement, which allows us to choose with more ‘freedom’ the pair of v and w for our vehicle.
The problem with this class is that to make it more realistic you can’t choose the linear speed of the vehicle, but you control the linear acceleration. In the first tests and using negative values, I didn’t see any change in its direction, until I found the speed value and after a little testing, I could see perfectly how the vehicle decelerates and then manages to go backwards.
Within the control class we have an scenario for each chassis, in our case we have used the chassis box, in which we have simply added a debugg line.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
if isinstance(chassis, BoxChassis):
# Since BoxChassis does not use pybullet for force-to-motion computations (only collision detection),
# we have to update the position and other state here (instead of pybullet.stepSimulation()).
heading_vec = radians_to_vec(vehicle.heading)
dpos = heading_vec * vehicle.speed * dt
# Debugging outputs
new_pose = Pose(
position=vehicle.position + np.append(dpos, 0.0),
orientation=fast_quaternion_from_angle(target_heading),
)
target_speed = vehicle.speed + acceleration * dt
# Debugging
print(f"Target Speed: {target_speed}")
vehicle.control(new_pose, target_speed, dt)
elif isinstance(chassis, AckermannChassis):
# code ...
else:
raise Exception("unsupported chassis type")
Sensor measurements
The current speed of the vehicle and the current position have been extracted, there are also relevant values such as whether there has been a collision or the lane position.
observations:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
{
'active': 1,
'steps_completed': 2,
'distance_travelled': 1.6576842,
'ego_vehicle_state': {
'angular_velocity': array([0., 0., 0.], dtype=float32),
'box': array([3.68, 1.47, 1.4 ], dtype=float32),
'heading': 1.1008694,
'lane_id': '445633931_0',
'lane_index': 0,
'linear_velocity': array([-13.079792 , 6.6428866, 0. ], dtype=float32),
'position': array([177.16279767, 86.23460276, 0. ]),
'speed': 14.67,
'steering': nan,
'yaw_rate': 0.0,
'mission': {
'goal_position': array([0., 0., 0.])
},
'angular_acceleration': array([0., 0., 0.], dtype=float32),
'angular_jerk': array([0., 0., 0.], dtype=float32),
'lane_position': array([156.01036, 0. , 0. ], dtype=float32),
'linear_acceleration': array([17.832027, -9.056423, 0. ], dtype=float32),
'linear_jerk': array([0., 0., 0.], dtype=float32)
},
'events': {
'interest_done': 0,
'agents_alive_done': 0,
'collisions': 0,
'not_moving': 0,
'off_road': 0,
'off_route': 0,
'on_shoulder': 0,
'reached_goal': 0,
'reached_max_episode_steps': 0,
'wrong_way': 0
}
}
info:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
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
90
91
92
93
94
95
96
97
98
99
100
101
102
103
{
"score": 1.6576841528508734,
"env_obs": {
"dt": 0.1,
"step_count": 2,
"steps_completed": 2,
"elapsed_sim_time": 0.3,
"events": {
"collisions": [],
"off_road": false,
"off_route": false,
"on_shoulder": false,
"wrong_way": false,
"not_moving": false,
"reached_goal": false,
"reached_max_episode_steps": false,
"agents_alive_done": false,
"interest_done": false
},
"ego_vehicle_state": {
"id": "Agent",
"position": [177.16279767447142, 86.23460275587868, 0.0],
"bounding_box": {
"length": 3.68,
"width": 1.47,
"height": 1.4
},
"heading": 1.1008693654767012,
"speed": 14.670000000000002,
"steering": null,
"yaw_rate": 0.0,
"road_id": "445633931",
"lane_id": "445633931_0",
"lane_index": 0,
"mission": {
"start": {
"position": [177.00855064, 86.31294082],
"heading": 1.1008693654767012,
"from_front_bumper": true
},
"goal": "EndlessGoal",
"route_vias": [],
"start_time": 0.1,
"entry_tactic": {
"start_time": 9223372036854775807,
"wait_to_hijack_limit_s": 0,
"zone": null,
"exclusion_prefixes": [],
"default_entry_speed": null,
"condition": {
"literal": "TRUE"
}
},
"via": [],
"vehicle_spec": null
},
"linear_velocity": [-13.079792010385955, 6.6428864934637994, 0.0],
"angular_velocity": [0.0, 0.0, 0.0],
"linear_acceleration": [17.832027280689786, -9.056423303972467, 0.0],
"angular_acceleration": [0.0, 0.0, 0.0],
"linear_jerk": [0.0, 0.0, 0.0],
"angular_jerk": [0.0, 0.0, 0.0],
"lane_position": {
"s": 156.01035380571716,
"t": 0.0,
"h": 0
}
},
"under_this_agent_control": true,
"neighborhood_vehicle_states": [],
"waypoint_paths": [
[
{
"pos": [177.16279767, 86.23460276],
"heading": 1.1008693654767063,
"lane_id": "445633931_0",
"lane_width": 3.2,
"speed_limit": 16.67,
"lane_index": 0,
"lane_offset": 155.8676865435755
},
{
"pos": [176.21315503, 86.71690158],
"heading": 1.1231176453367413,
"lane_id": "445633931_0",
"lane_width": 3.2,
"speed_limit": 16.67,
"lane_index": 0,
"lane_offset": 156.98893613485322
},
{
"pos": [175.26351238, 87.19920041],
"heading": 1.1453659251967763,
"lane_id": "445633931_0",
"lane_width": 3.2,
"speed_limit": 16.67,
"lane_index": 0,
"lane_offset": 158.11018572613094
}
]
]
}
}
Example:
1
2
3
# Extracting linear vel and position
print(f"Actual Speed: {observation['ego_vehicle_state']['speed']}")
print(f"Actual Pos: {observation['ego_vehicle_state']['position']}")
This post is licensed under CC BY 4.0 by the author.