def perform_action(sim, agent_id, vehicle, action, controller_state,
sensor_state, action_space):
if action is None:
return
if action_space == ActionSpaceType.Continuous:
vehicle.control(
throttle=np.clip(action[0], 0.0, 1.0),
brake=np.clip(action[1], 0.0, 1.0),
steering=np.clip(action[2], -1, 1),
)
elif action_space == ActionSpaceType.ActuatorDynamic:
ActuatorDynamicController.perform_action(vehicle,
action,
controller_state,
dt_sec=sim.timestep_sec)
elif action_space == ActionSpaceType.Trajectory:
TrajectoryTrackingController.perform_trajectory_tracking_PD(
action,
vehicle,
controller_state,
dt_sec=sim.timestep_sec,
)
elif action_space == ActionSpaceType.MPC:
TrajectoryTrackingController.perform_trajectory_tracking_MPC(
action, vehicle, controller_state, sim.timestep_sec)
elif action_space == ActionSpaceType.LaneWithContinuousSpeed:
LaneFollowingController.perform_lane_following(
sim,
agent_id,
vehicle,
controller_state,
sensor_state,
action[0],
action[1],
)
elif action_space == ActionSpaceType.Lane:
perform_lane_following = partial(
LaneFollowingController.perform_lane_following,
sim=sim,
agent_id=agent_id,
vehicle=vehicle,
controller_state=controller_state,
sensor_state=sensor_state,
)
# 12.5 m/s (45 km/h) is used as the nominal speed for lane change.
# For keep_lane, the nominal speed is set to 15 m/s (54 km/h).
if action == "keep_lane":
perform_lane_following(target_speed=15, lane_change=0)
elif action == "slow_down":
perform_lane_following(target_speed=0, lane_change=0)
elif action == "change_lane_left":
perform_lane_following(target_speed=12.5, lane_change=1)
elif action == "change_lane_right":
perform_lane_following(target_speed=12.5, lane_change=-1)
else:
raise ValueError(
f"perform_action(action_space={action_space}, ...) has failed "
"inside controller")
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",
)
def perform_action(
sim,
agent_id,
vehicle,
action,
controller_state,
sensor_state,
action_space,
vehicle_type,
):
print("in controller")
if action is None:
return
if vehicle_type == "bus":
assert action_space == ActionSpaceType.Trajectory
if action_space == ActionSpaceType.Continuous:
vehicle.control(
throttle=np.clip(action[0], 0.0, 1.0),
brake=np.clip(action[1], 0.0, 1.0),
steering=np.clip(action[2], -1, 1),
)
elif action_space == ActionSpaceType.ActuatorDynamic:
ActuatorDynamicController.perform_action(vehicle,
action,
controller_state,
dt_sec=sim.timestep_sec)
elif action_space == ActionSpaceType.Trajectory:
TrajectoryTrackingController.perform_trajectory_tracking_PD(
action,
vehicle,
controller_state,
dt_sec=sim.timestep_sec,
)
elif action_space == ActionSpaceType.MPC:
TrajectoryTrackingController.perform_trajectory_tracking_MPC(
action, vehicle, controller_state, sim.timestep_sec)
elif action_space == ActionSpaceType.LaneWithContinuousSpeed:
change_lane_set = [-1, 0, 1]
assert action[1] in change_lane_set, "target lane is not valid"
print(f"in Controllers: speed {action[0]}; lane {action[1]}")
LaneFollowingController.perform_lane_following(
sim,
agent_id,
vehicle,
controller_state,
sensor_state,
action[0],
int(action[1]),
)
elif action_space == ActionSpaceType.AnchorPoint:
# TODO: check action (AnchorPoint) is valid
# assert action is change_lane_set, "target lane is not valid"
target_speed = 15
LaneFollowingController.perform_lane_following_with_anchor(
sim,
agent_id,
vehicle,
controller_state,
sensor_state,
target_speed,
action,
)
elif action_space == ActionSpaceType.Lane:
print("in controller-dis")
perform_lane_following = partial(
LaneFollowingController.perform_lane_following,
sim=sim,
agent_id=agent_id,
vehicle=vehicle,
controller_state=controller_state,
sensor_state=sensor_state,
)
# 12.5 m/s (45 km/h) is used as the nominal speed for lane change.
# For keep_lane, the nominal speed is set to 15 m/s (54 km/h).
if action == "keep_lane":
perform_lane_following(target_speed=15, lane_change=0)
elif action == "slow_down":
perform_lane_following(target_speed=0, lane_change=0)
elif action == "change_lane_left":
perform_lane_following(target_speed=12.5, lane_change=1)
elif action == "change_lane_right":
perform_lane_following(target_speed=12.5, lane_change=-1)
# # 20.0 m/s (72 km/h) is used as the nominal speed for lane change.
# # For keep_lane, the nominal speed is set to 33.3 m/s (120 km/h).
# if action == "keep_lane":
# perform_lane_following(target_speed=33.3, lane_change=0)
# elif action == "slow_down":
# perform_lane_following(target_speed=0, lane_change=0)
# elif action == "change_lane_left":
# perform_lane_following(target_speed=20.0, lane_change=1)
# elif action == "change_lane_right":
# perform_lane_following(target_speed=20.0, lane_change=-1)
else:
raise ValueError(
f"perform_action(action_space={action_space}, ...) has failed "
"inside controller")
def perform_action(
sim,
agent_id,
vehicle,
action,
controller_state,
sensor_state,
action_space,
vehicle_type,
):
"""Calls control for the given vehicle based on a given action space and action.
Args:
sim:
A simulation instance.
agent_id:
An agent within the simulation that is associated with a vehicle.
vehicle:
A vehicle within the simulation that is associated with an agent.
action:
The action for the controller to perform.
controller_state:
The last vehicle controller state as relates to its action space.
sensor_state:
The state of a vehicle sensor as relates to vehicle sensors.
action_space:
The action space of the provided action.
vehicle_type:
Vehicle type information about the given vehicle.
"""
if action is None:
return
if vehicle_type == "bus":
assert action_space == ActionSpaceType.Trajectory
if action_space == ActionSpaceType.Continuous:
vehicle.control(
throttle=np.clip(action[0], 0.0, 1.0),
brake=np.clip(action[1], 0.0, 1.0),
steering=np.clip(action[2], -1, 1),
)
elif action_space == ActionSpaceType.ActuatorDynamic:
ActuatorDynamicController.perform_action(vehicle,
action,
controller_state,
dt_sec=sim.last_dt)
elif action_space == ActionSpaceType.Trajectory:
TrajectoryTrackingController.perform_trajectory_tracking_PD(
action,
vehicle,
controller_state,
dt_sec=sim.last_dt,
)
elif action_space == ActionSpaceType.MPC:
TrajectoryTrackingController.perform_trajectory_tracking_MPC(
action, vehicle, controller_state, sim.last_dt)
elif action_space == ActionSpaceType.LaneWithContinuousSpeed:
LaneFollowingController.perform_lane_following(
sim,
agent_id,
vehicle,
controller_state,
sensor_state,
action[0],
action[1],
)
elif action_space == ActionSpaceType.Lane:
perform_lane_following = partial(
LaneFollowingController.perform_lane_following,
sim=sim,
agent_id=agent_id,
vehicle=vehicle,
controller_state=controller_state,
sensor_state=sensor_state,
)
# 12.5 m/s (45 km/h) is used as the nominal speed for lane change.
# For keep_lane, the nominal speed is set to 15 m/s (54 km/h).
if action == "keep_lane":
perform_lane_following(target_speed=15, lane_change=0)
elif action == "slow_down":
perform_lane_following(target_speed=0, lane_change=0)
elif action == "change_lane_left":
perform_lane_following(target_speed=12.5, lane_change=1)
elif action == "change_lane_right":
perform_lane_following(target_speed=12.5, lane_change=-1)
elif action_space == ActionSpaceType.Imitation:
ImitationController.perform_action(sim.last_dt, vehicle, action)
else:
# Note: TargetPose and MultiTargetPose use a MotionPlannerProvider directly
raise ValueError(
f"perform_action(action_space={action_space}, ...) has failed "
"inside controller")
