def from_action_space(action_space, vehicle_pose, sim):
if action_space == ActionSpaceType.Lane:
# TAI: we should probably be fetching these waypoint through the mission planner
target_lane_id = sim.waypoints.closest_waypoint(
vehicle_pose, filter_from_count=4
).lane_id
return LaneFollowingControllerState(target_lane_id)
if action_space == ActionSpaceType.LaneWithContinuousSpeed:
# TAI: we should probably be fetching these waypoint through the mission planner
target_lane_id = sim.waypoints.closest_waypoint(
vehicle_pose, filter_from_count=4
).lane_id
return LaneFollowingControllerState(target_lane_id)
if action_space == ActionSpaceType.ActuatorDynamic:
return ActuatorDynamicControllerState()
if action_space == ActionSpaceType.Trajectory:
return TrajectoryTrackingControllerState()
if action_space == ActionSpaceType.MPC:
return TrajectoryTrackingControllerState()
# Other action spaces do not need a controller state object
return None
def from_action_space(action_space, vehicle_pose, sim):
"""Generate the appropriate controller state given an action space."""
if action_space in (
ActionSpaceType.Lane,
ActionSpaceType.LaneWithContinuousSpeed,
):
target_lane = sim.road_map.nearest_lane(vehicle_pose.point)
if not target_lane:
# This likely means this is a traffic history vehicle that is out-of-lane.
# If not, maybe increase radius in nearest_lane call?
raise ControllerOutOfLaneException(
"Controller has failed because actor is too far from lane for lane-following."
)
return LaneFollowingControllerState(target_lane.lane_id)
if action_space == ActionSpaceType.ActuatorDynamic:
return ActuatorDynamicControllerState()
if action_space == ActionSpaceType.Trajectory:
return TrajectoryTrackingControllerState()
if action_space == ActionSpaceType.MPC:
return TrajectoryTrackingControllerState()
# Other action spaces do not need a controller state object
return None
def run(base, client, vehicle, plane_body_id, sliders, n_steps=1e6):
prev_friction_sum = None
controller_state = ActuatorDynamicControllerState()
for _ in range(n_steps):
if not client.isConnected():
print("Client got disconnected")
return
action = [
client.readUserDebugParameter(sliders["throttle"]),
client.readUserDebugParameter(sliders["brake"]),
client.readUserDebugParameter(sliders["steering"]),
]
ActuatorDynamicController.perform_action(vehicle,
action,
controller_state,
dt_sec=TIMESTEP_SEC)
client.stepSimulation()
vehicle.sync_to_panda3d()
showbase.taskMgr.step()
frictions_ = frictions(sliders)
if prev_friction_sum is not None and not math.isclose(
sum(frictions_.values()), prev_friction_sum):
print("Updating")
return # will reset and take us to the next episode
prev_friction_sum = sum(frictions_.values())
look_at(client, vehicle.position, top_down=True)
print(
"Speed: {:.2f} m/s, Position: {}, Heading:{:.2f}, Sumo-Heading:{:.2f}"
.format(
vehicle.speed,
vehicle.position,
vehicle.heading,
vehicle.heading.as_sumo,
),
end="\r",
)
def run(client, vehicle, plane_body_id, sliders, n_steps=1e6):
prev_friction_sum = None
controller_state = ActuatorDynamicControllerState()
for yi in range(n_steps):
if not client.isConnected():
print("Client got disconnected")
return
action = [
client.readUserDebugParameter(sliders["throttle"]),
client.readUserDebugParameter(sliders["brake"]),
client.readUserDebugParameter(sliders["steering"]),
]
# if (yi * TIMESTEP_SEC) > 2:
# action[0] = 1
# if (yi * TIMESTEP_SEC) > 5 and (yi * TIMESTEP_SEC) < 10:
# action[2] = 0.5
# if (yi * TIMESTEP_SEC) > 10 and (yi * TIMESTEP_SEC) < 15:
# action[2] = -0.5
# if (yi * TIMESTEP_SEC) > 15:
# raise Exception("time is more than 20")
# ActuatorDynamicController.perform_action(
# vehicle, action, controller_state, dt_sec=TIMESTEP_SEC
# )
ActuatorDynamicController.perform_action(
vehicle, action, controller_state, dt_sec=TIMESTEP_SEC
)
z_yaw = vehicle.chassis.velocity_vectors[1][2]
xx.append(vehicle.position[0])
yy.append(vehicle.position[1])
rvx.append(z_yaw * vehicle.chassis.longitudinal_lateral_speed[0])
vy.append(vehicle.chassis.longitudinal_lateral_speed[1])
latforce.append(
(1 / 2500)
* (
sum(vehicle.chassis._lat_forces[2:4])
+ math.cos(vehicle.chassis.steering)
* sum(vehicle.chassis._lat_forces[0:2])
- math.sin(vehicle.chassis.steering)
* sum(vehicle.chassis._lon_forces[0:2])
)
)
print("Lateral Forces:", vehicle.chassis._lat_forces)
print("Steering:", vehicle.chassis.steering)
print(
"Distribution:",
vehicle.chassis.front_rear_axle_CG_distance[1],
sum(vehicle.chassis._lat_forces[2:4]),
)
kk = 1.5
latmoment.append(
(-1 / 3150)
* (
(3 - kk) * sum(vehicle.chassis._lat_forces[2:4])
- kk
* (
+math.cos(vehicle.chassis.steering)
* sum(vehicle.chassis._lat_forces[0:2])
- math.sin(vehicle.chassis.steering)
* sum(vehicle.chassis._lon_forces[0:2])
)
)
)
speed.append(vehicle.speed)
yaw.append(vehicle.chassis.yaw_rate[2])
time.append(yi * TIMESTEP_SEC)
# print(client.getDynamicsInfo(vehicle._chassis._bullet_id,-1),"ppppppppppppppppp")
print(client.getLinkState(vehicle._chassis._bullet_id, 0), "ppppppppppppppppp")
# client.changeDynamics(vehicle._chassis._bullet_id,-1,lateralFriction=0)
# client.changeDynamics(plane_body_id,0,lateralFriction=0)
# client.changeDynamics(plane_body_id,-1,lateralFriction=0)
# for iii in range(7):
# client.changeDynamics(vehicle._chassis._bullet_id,iii,lateralFriction=0)
# print(client.getDynamicsInfo(vehicle._chassis._bullet_id,iii),"ppppppppppppppppp",iii)
ss = list(
client.getContactPoints(plane_body_id, vehicle._chassis._bullet_id, -1, 2)
)
ss1 = list(
client.getContactPoints(plane_body_id, vehicle._chassis._bullet_id, -1, 4)
)
ss2 = list(
client.getContactPoints(plane_body_id, vehicle._chassis._bullet_id, -1, 5)
)
ss3 = list(
client.getContactPoints(plane_body_id, vehicle._chassis._bullet_id, -1, 6)
)
for i in ss:
flN.append(i[9])
break
for i in ss1:
frN.append(i[9])
break
for i in ss2:
rlN.append(i[9])
break
for i in ss3:
rrN.append(i[9])
break
client.stepSimulation()
frictions_ = frictions(sliders)
if prev_friction_sum is not None and not math.isclose(
sum(frictions_.values()), prev_friction_sum
):
print("Updating")
return # will reset and take us to the next episode
prev_friction_sum = sum(frictions_.values())
look_at(client, vehicle.position, top_down=True)
print(
"Speed: {:.2f} m/s, Position: {}, Heading:{:.2f}, Sumo-Heading:{:.2f}".format(
vehicle.speed,
vehicle.position,
vehicle.heading,
vehicle.heading.as_sumo,
),
end="\r",
)