class EscapeFromJCMB(object, DirectObject):
def __init__(self):
print "Loading..."
self.init_window()
self.init_music()
self.init_bullet()
self.init_key()
self.load_world()
self.init_player()
self.init_objects()
render.prepareScene(base.win.getGsg())
print "Done"
self.start_physics()
def init_window(self):
# Hide the mouse cursor
props = WindowProperties()
props.setCursorHidden(True)
base.win.requestProperties(props)
def init_music(self):
music = base.loader.loadSfx("../data/snd/Bent_and_Broken.mp3")
music.play()
self.playferscream = base.loader.loadSfx("../data/snd/deadscrm.wav")
def init_bullet(self):
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
# debugNode = BulletDebugNode('Debug')
# debugNode.showWireframe(True)
# debugNode.showConstraints(True)
# debugNode.showBoundingBoxes(False)
# debugNode.showNormals(False)
# debugNP = render.attachNewNode(debugNode)
# debugNP.show()
# self.world.setDebugNode(debugNP.node())
alight = AmbientLight('alight')
alight.setColor(VBase4(0.05, 0.05, 0.05, 1))
alnp = render.attachNewNode(alight)
render.setLight(alnp)
def init_key(self):
# Stores the state of the keys, 1 for pressed and 0 for unpressed
self.key_state = {
'up': False,
'right': False,
'down': False,
'left': False
}
# Assign the key event handler
self.accept('w', self.set_key_state, ['up', True])
self.accept('w-up', self.set_key_state, ['up', False])
self.accept('d', self.set_key_state, ['right', True])
self.accept('d-up', self.set_key_state, ['right', False])
self.accept('s', self.set_key_state, ['down', True])
self.accept('s-up', self.set_key_state, ['down', False])
self.accept('a', self.set_key_state, ['left', True])
self.accept('a-up', self.set_key_state, ['left', False])
# Stores the state of the mouse buttons, 1 for pressed and 0 for unpressed
self.mouse_state = {'left_click': False, 'right_click': False}
# Assign the mouse click event handler
self.accept('mouse1', self.set_mouse_state, ['left_click', True])
self.accept('mouse1-up', self.set_mouse_state, ['left_click', False])
self.accept('mouse2', self.set_mouse_state, ['right_click', True])
self.accept('mouse2-up', self.set_mouse_state, ['right_click', False])
self.accept('z', self.print_pos)
# Esc
self.accept('escape', sys.exit)
def set_key_state(self, key, state):
self.key_state[key] = state
def set_mouse_state(self, button, state):
self.mouse_state[button] = state
def print_pos(self):
print self.playernp.getPos()
def egg_to_BulletTriangleMeshShape(self, modelnp):
mesh = BulletTriangleMesh()
for collisionNP in modelnp.findAllMatches('**/+CollisionNode'):
collisionNode = collisionNP.node()
for collisionPolygon in collisionNode.getSolids():
tri_points = collisionPolygon.getPoints()
mesh.addTriangle(tri_points[0], tri_points[1], tri_points[2])
shape = BulletTriangleMeshShape(mesh, dynamic=False)
return shape
def load_world(self):
stairwell = loader.loadModel('../data/mod/jcmbstairs.egg')
stairwell.reparentTo(render)
stairwell_shape = self.egg_to_BulletTriangleMeshShape(stairwell)
stairwellnode = BulletRigidBodyNode('stairwell')
stairwellnode.addShape(stairwell_shape)
self.world.attachRigidBody(stairwellnode)
def init_player(self):
# Stop the default mouse behaviour
base.disableMouse()
# Character has a capsule shape
shape = BulletCapsuleShape(0.2, 1, ZUp)
self.player = BulletRigidBodyNode('Player')
self.player.setMass(80.0)
self.player.addShape(shape)
self.playernp = render.attachNewNode(self.player)
self.playernp.setPos(0, 0, 1)
self.world.attachRigidBody(self.player)
self.player.setLinearSleepThreshold(0.0)
self.player.setAngularFactor(0.0)
self.player_speed = 3
self.player_is_grabbing = False
# self.head = BulletRigidBodyNode('Player Head')
# self.head.addShape(BulletSphereShape(0.2))
# self.head.setMass(10)
# self.head.setInertia(Vec3(1,1,1))
# self.head.setAngularFactor(1.0)
# self.head.setLinearFactor(0.0)
# self.headnp = self.playernp.attachNewNode(self.head)
# self.headnp.setPos(0,0,0)
# self.headnp.setCollideMask(BitMask32.allOff())
# self.world.attachRigidBody(self.head)
# Attach the camera to the player's head
base.camera.reparentTo(self.playernp)
# base.camera.setPos(0,0,.5)
base.camLens.setFov(80)
base.camLens.setNear(0.01)
# Make the torch and attach it to our character
torch = Spotlight('torch')
torch.setColor(VBase4(1, 1, 1, 1))
lens = PerspectiveLens()
lens.setFov(80)
lens.setNearFar(20, 100)
torch.setLens(lens)
self.torchnp = base.camera.attachNewNode(torch)
self.torchnp.setPos(0, 0, 0)
# Allow the world to be illuminated by our torch
render.setLight(self.torchnp)
self.cs = None
# Player's "hand" in the world
hand_model = loader.loadModel('../data/mod/hand.egg')
hand_model.setScale(1)
hand_model.setBillboardPointEye()
self.hand = BulletRigidBodyNode('Hand')
self.hand.addShape(BulletSphereShape(0.1))
self.hand.setLinearSleepThreshold(0.0)
self.hand.setLinearDamping(0.9999999)
self.hand.setAngularFactor(0.0)
self.hand.setMass(1.0)
self.world.attachRigidBody(self.hand)
self.handnp = render.attachNewNode(self.hand)
self.handnp.setCollideMask(BitMask32.allOff())
# hand_model.reparentTo(self.handnp)
# Create a text node to display object identification information and attach it to the player's "hand"
self.hand_text = TextNode('Hand Text')
self.hand_text.setText("Ch-ch-chek yoself befo yo rek yoself, bro.")
self.hand_text.setAlign(TextNode.ACenter)
self.hand_text.setTextColor(1, 1, 1, 1)
self.hand_text.setShadow(0.05, 0.05)
self.hand_text.setShadowColor(0, 0, 0, 1)
self.hand_text_np = render.attachNewNode(self.hand_text)
self.hand_text_np.setScale(0.03)
self.hand_text_np.setBillboardPointEye()
self.hand_text_np.hide()
# Disable the depth testing for the hand and the text - we always want these things on top, with no clipping
self.handnp.setDepthTest(False)
self.hand_text_np.setDepthTest(False)
# Add the player update task
taskMgr.add(self.update, 'update_player_task')
def init_objects(self):
# Make Playfer Box
shape = BulletBoxShape(Vec3(0.25, 0.25, 0.25))
node = BulletRigidBodyNode('Playfer Box')
node.setMass(110.0)
node.setFriction(1.0)
node.addShape(shape)
node.setAngularDamping(0.0)
np = render.attachNewNode(node)
np.setPos(-1.4, 1.7, -1.7)
self.world.attachRigidBody(node)
playferboxmodel = loader.loadModel('../data/mod/playferbox.egg')
playferboxmodel.reparentTo(np)
# Make Pendlepot
shape = BulletBoxShape(Vec3(0.2, 0.15, 0.1))
node = BulletRigidBodyNode('Pendlepot')
node.setMass(5.0)
node.setFriction(1.0)
node.addShape(shape)
node.setAngularDamping(0.0)
np = render.attachNewNode(node)
np.setPos(-1.4, 1.7, -1.5)
self.world.attachRigidBody(node)
pendlepotmodel = loader.loadModel('../data/mod/pendlepot.egg')
pendlepotmodel.reparentTo(np)
def update(self, task):
# Update camera orientation
md = base.win.getPointer(0)
mouse_x = md.getX()
mouse_y = md.getY()
centre_x = base.win.getXSize() / 2
centre_y = base.win.getYSize() / 2
if base.win.movePointer(0, centre_x, centre_y):
new_H = base.camera.getH() + (centre_x - mouse_x)
new_P = base.camera.getP() + (centre_y - mouse_y)
if new_P < -90:
new_P = -90
elif new_P > 90:
new_P = 90
base.camera.setH(new_H)
base.camera.setP(new_P)
# Update player position
speed = 3
self.player_is_moving = False
if (self.key_state["up"] == True):
self.player_is_moving = True
dir = 0
if (self.key_state["down"] == True):
self.player_is_moving = True
dir = 180
if (self.key_state["left"] == True):
self.player_is_moving = True
dir = 90
if (self.key_state["right"] == True):
self.player_is_moving = True
dir = 270
self.player.clearForces()
old_vel = self.player.getLinearVelocity()
new_vel = Vec3(0, 0, 0)
if self.player_is_moving == True:
new_vel.setX(-speed * math.sin(
(base.camera.getH() + dir) * 3.1415 / 180.0))
new_vel.setY(speed * math.cos(
(base.camera.getH() + dir) * 3.1415 / 180.0))
timescale = 0.001
linear_force = (new_vel - old_vel) / (timescale)
linear_force.setZ(0.0)
self.player.applyCentralForce(linear_force)
if self.player_is_grabbing == False:
new_hand_pos = LPoint3f(
render.getRelativePoint(base.camera, Vec3(0, 0.2, 0)))
self.handnp.setPos(new_hand_pos)
else:
new_hand_pos = LPoint3f(
render.getRelativePoint(base.camera, Vec3(0, 0.5, 0)))
diff = new_hand_pos - self.handnp.getPos()
self.hand.applyCentralForce(diff * 1000 -
self.hand.getLinearVelocity() * 100)
if diff.length() > .5:
self.player.setLinearVelocity(Vec3(0, 0, 0))
# Identify what lies beneath the player's hand (unless player is holding something)
if self.player_is_grabbing == False:
ray_from = self.playernp.getPos()
ray_to = LPoint3f(
render.getRelativePoint(base.camera, Vec3(0, 1, 0)))
result = self.world.rayTestClosest(ray_from, ray_to)
if result.hasHit() == True:
self.hand_text.setText(result.getNode().getName())
self.hand_text_np.setPos(result.getHitPos())
self.hand_text_np.show()
# If player clicks, grab the object by the nearest point (as chosen by ray)
if self.mouse_state["left_click"] == True:
if result.getNode().getNumChildren() == 1:
obj = NodePath(result.getNode().getChild(0))
if self.player_is_grabbing == False:
self.player_is_grabbing = True
# Find the position of contact in terms of the object's local coordinates.
# Parent the player's hand to the grabbed object at that position.
pos = obj.getRelativePoint(render,
result.getHitPos())
self.grabbed_node = result.getNode()
self.grabbed_node.setActive(True)
print self.grabbed_node
frameA = TransformState.makePosHpr(
Vec3(0, 0, 0), Vec3(0, 0, 0))
frameB = TransformState.makePosHpr(
Vec3(0, 0, 0), Vec3(0, 0, 0))
swing1 = 20 # degrees
swing2 = 20 # degrees
twist = 20 # degrees
self.cs = BulletConeTwistConstraint(
self.hand, result.getNode(), frameA, frameB)
self.cs.setLimit(swing1, swing2, twist)
self.world.attachConstraint(self.cs)
# Stop the held object swinging all over the place
result.getNode().setAngularDamping(0.7)
else:
self.hand_text_np.hide()
self.player_is_grabbing = False
if self.mouse_state["left_click"] == False:
self.player_is_grabbing = False
if self.cs != None:
self.world.remove_constraint(self.cs)
self.cs = None
self.grabbed_node.setAngularDamping(0.0)
if self.player_is_grabbing == True and self.mouse_state[
"right_click"] == True:
self.world.remove_constraint(self.cs)
self.cs = None
self.grabbed_node.setAngularDamping(0.0)
self.grabbed_node.setActive(True)
self.grabbed_node.applyCentralImpulse(1000, 0, 0)
if self.player_is_grabbing == True:
self.hand_text_np.hide()
return task.cont
def update_physics(self, task):
dt = globalClock.getDt()
self.world.doPhysics(dt)
return task.cont
def start_physics(self):
taskMgr.add(self.update_physics, 'update_physics')
class Drone:
"""
Class for a single drone in the simulation.
"""
RIGID_BODY_MASS = .5 # Mass of physics object
COLLISION_SPHERE_RADIUS = .1 # Radius of the bullet collision sphere around the drone
FRICTION = 0 # No friction between drone and objects, not really necessary
TARGET_FORCE_MULTIPLIER = 1 # Allows to change influence of the force applied to get to the target
AVOIDANCE_FORCE_MULTIPLIER = 10 # Allows to change influence of the force applied to avoid collisions
TARGET_PROXIMITY_RADIUS = .5 # Drone reduces speed when in proximity of a target
AVOIDANCE_PROXIMITY_RADIUS = .6 # Distance when an avoidance manoeuvre has to be done
# TODO: Check if other values are better here (and find out what they really do as well..)
LINEAR_DAMPING = 0.95
LINEAR_SLEEP_THRESHOLD = 0
def __init__(self, manager, number):
"""
Initialises the drone as a bullet and panda object.
:param manager: The drone manager creating this very drone.
"""
self.manager = manager # Drone manager handling this drone
self.base = manager.base # Simulation
self.crazyflie = None # object of real drone, if connected to one
self.debug = False # If debugging info should be given
self.in_flight = False # If currently in flight
self.number = number # Number of drone in list
# Every drone has its own vector to follow if an avoidance manouver has to be done
self.avoidance_vector = LVector3f(random.uniform(-1, 1),
random.uniform(-1, 1),
random.uniform(-1, 1)).normalize()
# Create bullet rigid body for drone
drone_collision_shape = BulletSphereShape(self.COLLISION_SPHERE_RADIUS)
self.drone_node_bullet = BulletRigidBodyNode("RigidSphere")
self.drone_node_bullet.addShape(drone_collision_shape)
self.drone_node_bullet.setMass(self.RIGID_BODY_MASS)
# Set some values for the physics object
self.drone_node_bullet.setLinearSleepThreshold(
self.LINEAR_SLEEP_THRESHOLD)
self.drone_node_bullet.setFriction(self.FRICTION)
self.drone_node_bullet.setLinearDamping(self.LINEAR_DAMPING)
# Attach to the simulation
self.drone_node_panda = self.base.render.attachNewNode(
self.drone_node_bullet)
# ...and physics engine
self.base.world.attachRigidBody(self.drone_node_bullet)
# Add a model to the drone to be actually seen in the simulation
drone_model = self.base.loader.loadModel(
"models/drones/drone_florian.egg")
drone_model.setScale(0.2)
drone_model.reparentTo(self.drone_node_panda)
# Set the position and target position to their default (origin)
default_position = LPoint3f(0, 0, 0)
self.drone_node_panda.setPos(default_position)
self.target_position = default_position
# Create a line renderer to draw a line from center to target point
self.line_creator = LineSegs()
# Then draw a default line so that the update function works as expected (with the removal)
self.target_line_node = self.base.render.attachNewNode(
self.line_creator.create(False))
# Create node for text
self.drone_text_node_panda = None
def get_pos(self) -> LPoint3f:
"""
Get the position of the drone.
:return: Position of the drone as an LPoint3 object
"""
return self.drone_node_panda.getPos()
def set_pos(self, position: LPoint3f):
"""
Set position of the drone.
This directly sets the drone to that position, without any transition or flight.
:param position: The position the drone is supposed to be at.
"""
self.drone_node_panda.setPos(position)
def get_target(self) -> LPoint3f:
"""
Get the current target position of the drone.
:return: The target position as a LPoint3 object.
"""
return self.target_position
def set_target(self, position: LPoint3f):
"""
Set the target position of the drone.
:param position: The target position to be set.
"""
self.target_position = position
def set_debug(self, active):
"""
De-/activate debug information such as lines showing forces and such.
:param active: If debugging should be turned on or off.
"""
self.debug = active
if active:
# Create a line so the updater can update it
self.target_line_node = self.base.render.attachNewNode(
self.line_creator.create(False))
# Write address of drone above model
self._draw_cf_name(True)
else:
self.target_line_node.removeNode()
self._draw_cf_name(False)
def update(self):
"""
Update the drone and its forces.
"""
# Update the force needed to get to the target
self._update_target_force()
# Update the force needed to avoid other drones, if any
self._update_avoidance_force()
# Combine all acting forces and normalize them to one acting force
self._combine_forces()
# Update the line drawn to the current target
if self.debug:
self._draw_target_line()
# Update real drone if connected to one
if self.crazyflie is not None and self.in_flight:
current_pos = self.get_pos()
self.crazyflie.cf.commander.send_position_setpoint(
current_pos.y, -current_pos.x, current_pos.z, 0)
# print("Update!")
def _update_target_force(self):
"""
Calculates force needed to get to the target and applies it.
"""
distance = (self.get_target() - self.get_pos()
) # Calculate distance to fly
# Normalise distance to get an average force for all drones, unless in close proximity of target,
# then slowing down is encouraged and the normalisation is no longer needed
# If normalisation would always take place overshoots would occur
if distance > self.TARGET_PROXIMITY_RADIUS:
distance = distance.normalized()
# Apply to drone (with force multiplier in mind)
self.drone_node_bullet.applyCentralForce(distance *
self.TARGET_FORCE_MULTIPLIER)
def _update_avoidance_force(self):
"""
Applies a force to drones that are to close to each other and to avoid crashes.
"""
# Save all drones in near proximity with their distance
near = []
for drone in self.manager.drones:
distance = len(drone.get_pos() - self.get_pos())
if 0 < distance < self.AVOIDANCE_PROXIMITY_RADIUS: # Check dist > 0 to prevent drone from detecting itself
near.append(drone)
# Calculate and apply forces for every opponent
for opponent in near:
# Vector to other drone
opponent_vector = opponent.get_pos() - self.get_pos()
# Multiplier to maximise force when opponent gets closer
multiplier = self.AVOIDANCE_PROXIMITY_RADIUS - len(opponent_vector)
# Calculate a direction follow (multipliers found by testing)
avoidance_direction = self.avoidance_vector * 2 - opponent_vector.normalized(
) * 10
avoidance_direction.normalize()
# Apply avoidance force
self.drone_node_bullet.applyCentralForce(
avoidance_direction * multiplier *
self.AVOIDANCE_FORCE_MULTIPLIER)
def _combine_forces(self):
"""
Combine all acting forces to one normalised force.
"""
total_force = self.drone_node_bullet.getTotalForce()
# Only do so if force is sufficiently big to retain small movements
if total_force.length() > 2:
self.drone_node_bullet.clearForces()
self.drone_node_bullet.applyCentralForce(total_force.normalized())
def destroy(self):
"""
Detach drone from the simulation and physics engine, then destroy object.
"""
self.drone_node_panda.removeNode()
self.base.world.removeRigidBody(self.drone_node_bullet)
self.target_line_node.removeNode()
if self.debug:
self.drone_text_node_panda.removeNode()
def _draw_target_line(self):
"""
Draw a line from the center to the target position in the simulation.
"""
# Remove the former line
self.target_line_node.removeNode()
# Create a new one
self.line_creator.setColor(1.0, 0.0, 0.0, 1.0)
self.line_creator.moveTo(self.get_pos())
self.line_creator.drawTo(self.target_position)
line = self.line_creator.create(False)
# And attach it to the renderer
self.target_line_node = self.base.render.attachNewNode(line)
def _draw_cf_name(self, draw):
"""
Show the address of the connected Craziefly, if there is one, above the model.
"""
if draw:
address = 'Not connected'
if self.crazyflie is not None:
address = self.crazyflie.cf.link_uri
text = TextNode('droneInfo')
text.setText(str(self.number) + '\n' + address)
text.setAlign(TextNode.ACenter)
self.drone_text_node_panda = self.base.render.attachNewNode(text)
self.drone_text_node_panda.setScale(.1)
self.drone_text_node_panda.setPos(self.drone_node_panda, 0, 0, .3)
else:
self.drone_text_node_panda.removeNode()
class CarEnv(DirectObject):
def __init__(self, params={}):
self._params = params
if 'random_seed' in self._params:
np.random.seed(self._params['random_seed'])
self._use_vel = self._params.get('use_vel', True)
self._run_as_task = self._params.get('run_as_task', False)
self._do_back_up = self._params.get('do_back_up', False)
self._use_depth = self._params.get('use_depth', False)
self._use_back_cam = self._params.get('use_back_cam', False)
self._collision_reward_only = self._params.get('collision_reward_only',
False)
self._collision_reward = self._params.get('collision_reward', -10.0)
self._obs_shape = self._params.get('obs_shape', (64, 36))
self._steer_limits = params.get('steer_limits', (-30., 30.))
self._speed_limits = params.get('speed_limits', (-4.0, 4.0))
self._motor_limits = params.get('motor_limits', (-0.5, 0.5))
self._fixed_speed = (self._speed_limits[0] == self._speed_limits[1]
and self._use_vel)
if not self._params.get('visualize', False):
loadPrcFileData('', 'window-type offscreen')
# Defines base, render, loader
try:
ShowBase()
except:
pass
base.setBackgroundColor(0.0, 0.0, 0.0, 1)
# World
self._worldNP = render.attachNewNode('World')
self._world = BulletWorld()
self._world.setGravity(Vec3(0, 0, -9.81))
self._dt = params.get('dt', 0.25)
self._step = 0.05
# Vehicle
shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25))
ts = TransformState.makePos(Point3(0., 0., 0.25))
self._vehicle_node = BulletRigidBodyNode('Vehicle')
self._vehicle_node.addShape(shape, ts)
self._mass = self._params.get('mass', 10.)
self._vehicle_node.setMass(self._mass)
self._vehicle_node.setDeactivationEnabled(False)
self._vehicle_node.setCcdSweptSphereRadius(1.0)
self._vehicle_node.setCcdMotionThreshold(1e-7)
self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node)
self._world.attachRigidBody(self._vehicle_node)
self._vehicle = BulletVehicle(self._world, self._vehicle_node)
self._vehicle.setCoordinateSystem(ZUp)
self._world.attachVehicle(self._vehicle)
self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07)
self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07)
# Camera
size = self._params.get('size', [160, 90])
hfov = self._params.get('hfov', 120)
near_far = self._params.get('near_far', [0.1, 100.])
self._camera_sensor = Panda3dCameraSensor(base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='front cam')
self._camera_node = self._camera_sensor.cam
self._camera_node.setPos(0.0, 0.5, 0.375)
self._camera_node.lookAt(0.0, 6.0, 0.0)
self._camera_node.reparentTo(self._vehicle_pointer)
if self._use_back_cam:
self._back_camera_sensor = Panda3dCameraSensor(
base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='back cam')
self._back_camera_node = self._back_camera_sensor.cam
self._back_camera_node.setPos(0.0, -0.5, 0.375)
self._back_camera_node.lookAt(0.0, -6.0, 0.0)
self._back_camera_node.reparentTo(self._vehicle_pointer)
# Car Simulator
self._des_vel = None
self._setup()
# Input
self.accept('escape', self._doExit)
self.accept('r', self.reset)
self.accept('f1', self._toggleWireframe)
self.accept('f2', self._toggleTexture)
self.accept('f3', self._view_image)
self.accept('f5', self._doScreenshot)
self.accept('q', self._forward_0)
self.accept('w', self._forward_1)
self.accept('e', self._forward_2)
self.accept('a', self._left)
self.accept('s', self._stop)
self.accept('x', self._backward)
self.accept('d', self._right)
self.accept('m', self._mark)
self._steering = 0.0 # degree
self._engineForce = 0.0
self._brakeForce = 0.0
self._env_time_step = 0
self._p = self._params.get('p', 1.25)
self._d = self._params.get('d', 0.0)
self._last_err = 0.0
self._curr_time = 0.0
self._accelClamp = self._params.get('accelClamp', 0.5)
self._engineClamp = self._accelClamp * self._mass
self._collision = False
self._setup_spec()
self.spec = EnvSpec(observation_im_space=self.observation_im_space,
action_space=self.action_space,
action_selection_space=self.action_selection_space,
observation_vec_spec=self.observation_vec_spec,
action_spec=self.action_spec,
action_selection_spec=self.action_selection_spec,
goal_spec=self.goal_spec)
if self._run_as_task:
self._mark_d = 0.0
taskMgr.add(self._update_task, 'updateWorld')
base.run()
def _setup_spec(self):
self.action_spec = OrderedDict()
self.action_selection_spec = OrderedDict()
self.observation_vec_spec = OrderedDict()
self.goal_spec = OrderedDict()
self.action_spec['steer'] = Box(low=-45., high=45.)
self.action_selection_spec['steer'] = Box(low=self._steer_limits[0],
high=self._steer_limits[1])
if self._use_vel:
self.action_spec['speed'] = Box(low=-4., high=4.)
self.action_space = Box(low=np.array([
self.action_spec['steer'].low[0],
self.action_spec['speed'].low[0]
]),
high=np.array([
self.action_spec['steer'].high[0],
self.action_spec['speed'].high[0]
]))
self.action_selection_spec['speed'] = Box(
low=self._speed_limits[0], high=self._speed_limits[1])
self.action_selection_space = Box(
low=np.array([
self.action_selection_spec['steer'].low[0],
self.action_selection_spec['speed'].low[0]
]),
high=np.array([
self.action_selection_spec['steer'].high[0],
self.action_selection_spec['speed'].high[0]
]))
else:
self.action_spec['motor'] = Box(low=-self._accelClamp,
high=self._accelClamp)
self.action_space = Box(low=np.array([
self.action_spec['steer'].low[0],
self.action_spec['motor'].low[0]
]),
high=np.array([
self.action_spec['steer'].high[0],
self.action_spec['motor'].high[0]
]))
self.action_selection_spec['motor'] = Box(
low=self._motor_limits[0], high=self._motor_limits[1])
self.action_selection_space = Box(
low=np.array([
self.action_selection_spec['steer'].low[0],
self.action_selection_spec['motor'].low[0]
]),
high=np.array([
self.action_selection_spec['steer'].high[0],
self.action_selection_spec['motor'].high[0]
]))
assert (np.logical_and(
self.action_selection_space.low >= self.action_space.low - 1e-4,
self.action_selection_space.high <=
self.action_space.high + 1e-4).all())
self.observation_im_space = Box(low=0,
high=255,
shape=tuple(
self._get_observation()[0].shape))
self.observation_vec_spec['coll'] = Discrete(1)
self.observation_vec_spec['heading'] = Box(low=0, high=2 * 3.14)
self.observation_vec_spec['speed'] = Box(low=-4.0, high=4.0)
@property
def _base_dir(self):
return os.path.join(os.path.dirname(os.path.abspath(__file__)),
'models')
@property
def horizon(self):
return np.inf
@property
def max_reward(self):
return np.inf
# _____HANDLER_____
def _doExit(self):
sys.exit(1)
def _toggleWireframe(self):
base.toggleWireframe()
def _toggleTexture(self):
base.toggleTexture()
def _doScreenshot(self):
base.screenshot('Bullet')
def _forward_0(self):
self._des_vel = 1
self._brakeForce = 0.0
def _forward_1(self):
self._des_vel = 2
self._brakeForce = 0.0
def _forward_2(self):
self._des_vel = 4
self._brakeForce = 0.0
def _stop(self):
self._des_vel = 0.0
self._brakeForce = 0.0
def _backward(self):
self._des_vel = -4
self._brakeForce = 0.0
def _right(self):
self._steering = np.min([np.max([-30, self._steering - 5]), 0.0])
def _left(self):
self._steering = np.max([np.min([30, self._steering + 5]), 0.0])
def _view_image(self):
from matplotlib import pyplot as plt
image = self._camera_sensor.observe()[0]
if self._use_depth:
plt.imshow(image[:, :, 0], cmap='gray')
else:
def rgb2gray(rgb):
return np.dot(rgb[..., :3], [0.299, 0.587, 0.114])
image = rgb2gray(image)
im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA
) # TODO how does this deal with aspect ratio
plt.imshow(im.astype(np.uint8), cmap='Greys_r')
plt.show()
def _mark(self):
self._mark_d = 0.0
# Setup
def _setup(self):
self._setup_map()
self._place_vehicle()
self._setup_light()
self._setup_restart_pos()
def _setup_map(self):
if hasattr(self, '_model_path'):
# Collidable objects
self._setup_collision_object(self._model_path)
else:
ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
ground.node().addShape(shape)
ground.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(ground.node())
def _setup_collision_object(self,
path,
pos=(0.0, 0.0, 0.0),
hpr=(0.0, 0.0, 0.0),
scale=1):
visNP = loader.loadModel(path)
visNP.clearModelNodes()
visNP.reparentTo(render)
visNP.setPos(pos[0], pos[1], pos[2])
visNP.setHpr(hpr[0], hpr[1], hpr[2])
visNP.set_scale(scale, scale, scale)
bodyNPs = BulletHelper.fromCollisionSolids(visNP, True)
for bodyNP in bodyNPs:
bodyNP.reparentTo(render)
bodyNP.setPos(pos[0], pos[1], pos[2])
bodyNP.setHpr(hpr[0], hpr[1], hpr[2])
bodyNP.set_scale(scale, scale, scale)
if isinstance(bodyNP.node(), BulletRigidBodyNode):
bodyNP.node().setMass(0.0)
bodyNP.node().setKinematic(True)
bodyNP.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(bodyNP.node())
else:
print("Issue")
def _setup_restart_pos(self):
self._restart_index = 0
self._restart_pos = self._default_restart_pos()
def _next_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
pos_hpr = self._restart_pos[self._restart_index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _setup_light(self):
# alight = AmbientLight('ambientLight')
# alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
# alightNP = render.attachNewNode(alight)
# render.clearLight()
# render.setLight(alightNP)
pass
# Vehicle
def _default_pos(self):
return (0.0, 0.0, 0.3)
def _default_hpr(self):
return (0.0, 0.0, 0.0)
def _default_restart_pos(self):
return [self._default_pos() + self._default_hpr()]
def _get_speed(self):
vel = self._vehicle.getCurrentSpeedKmHour() / 3.6
return vel
def _get_heading(self):
h = np.array(self._vehicle_pointer.getHpr())[0]
ori = h * (pi / 180.)
while (ori > 2 * pi):
ori -= 2 * pi
while (ori < 0):
ori += 2 * pi
return ori
def _update(self, dt=1.0, coll_check=True):
self._vehicle.setSteeringValue(self._steering, 0)
self._vehicle.setSteeringValue(self._steering, 1)
self._vehicle.setBrake(self._brakeForce, 0)
self._vehicle.setBrake(self._brakeForce, 1)
self._vehicle.setBrake(self._brakeForce, 2)
self._vehicle.setBrake(self._brakeForce, 3)
if dt >= self._step:
# TODO maybe change number of timesteps
# for i in range(int(dt/self._step)):
if self._des_vel is not None:
vel = self._get_speed()
err = self._des_vel - vel
d_err = (err - self._last_err) / self._step
self._last_err = err
self._engineForce = np.clip(self._p * err + self._d * d_err,
-self._accelClamp,
self._accelClamp) * self._mass
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
for _ in range(int(dt / self._step)):
self._world.doPhysics(self._step, 1, self._step)
self._collision = self._is_contact()
elif self._run_as_task:
self._curr_time += dt
if self._curr_time > 0.05:
if self._des_vel is not None:
vel = self._get_speed()
self._mark_d += vel * self._curr_time
print(vel, self._mark_d, self._is_contact())
err = self._des_vel - vel
d_err = (err - self._last_err) / 0.05
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._curr_time = 0.0
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(dt, 1, dt)
self._collision = self._is_contact()
else:
raise ValueError(
"dt {0} s is too small for velocity control".format(dt))
def _stop_car(self):
self._steering = 0.0
self._engineForce = 0.0
self._vehicle.setSteeringValue(0.0, 0)
self._vehicle.setSteeringValue(0.0, 1)
self._vehicle.applyEngineForce(0.0, 0)
self._vehicle.applyEngineForce(0.0, 1)
self._vehicle.applyEngineForce(0.0, 2)
self._vehicle.applyEngineForce(0.0, 3)
if self._des_vel is not None:
self._des_vel = 0
self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0))
self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0))
for i in range(self._vehicle.getNumWheels()):
wheel = self._vehicle.getWheel(i)
wheel.setRotation(0.0)
self._vehicle_node.clearForces()
def _place_vehicle(self, pos=None, hpr=None):
if pos is None:
pos = self._default_pos()
if hpr is None:
hpr = self._default_hpr()
self._vehicle_pointer.setPos(pos[0], pos[1], pos[2])
self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2])
self._stop_car()
def _addWheel(self, pos, front, radius=0.25):
wheel = self._vehicle.createWheel()
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(1e2)
wheel.setRollInfluence(0.1)
# Task
def _update_task(self, task):
dt = globalClock.getDt()
self._update(dt=dt)
self._get_observation()
return task.cont
# Helper functions
def _get_observation(self):
self._obs = self._camera_sensor.observe()
observation = []
observation.append(self.process(self._obs[0], self._obs_shape))
if self._use_back_cam:
self._back_obs = self._back_camera_sensor.observe()
observation.append(self.process(self._back_obs[0],
self._obs_shape))
observation_im = np.concatenate(observation, axis=2)
coll = self._collision
heading = self._get_heading()
speed = self._get_speed()
observation_vec = np.array([coll, heading, speed])
return observation_im, observation_vec
def _get_goal(self):
return np.array([])
def process(self, image, obs_shape):
if self._use_depth:
im = np.reshape(image, (image.shape[0], image.shape[1]))
if im.shape != obs_shape:
im = cv2.resize(im, obs_shape, interpolation=cv2.INTER_AREA)
return im.astype(np.uint8)
else:
image = np.dot(image[..., :3], [0.299, 0.587, 0.114])
im = cv2.resize(image, obs_shape, interpolation=cv2.INTER_AREA
) #TODO how does this deal with aspect ratio
# TODO might not be necessary
im = np.expand_dims(im, 2)
return im.astype(np.uint8)
def _get_reward(self):
if self._collision_reward_only:
if self._collision:
reward = self._collision_reward
else:
reward = 0.0
else:
if self._collision:
reward = self._collision_reward
else:
reward = self._get_speed()
assert (reward <= self.max_reward)
return reward
def _get_done(self):
return self._collision
def _get_info(self):
info = {}
info['pos'] = np.array(self._vehicle_pointer.getPos())
info['hpr'] = np.array(self._vehicle_pointer.getHpr())
info['vel'] = self._get_speed()
info['coll'] = self._collision
info['env_time_step'] = self._env_time_step
return info
def _back_up(self):
assert (self._use_vel)
# logger.debug('Backing up!')
self._params['back_up'] = self._params.get('back_up', {})
back_up_vel = self._params['back_up'].get('vel', -1.0)
self._des_vel = back_up_vel
back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0))
self._steering = np.random.uniform(*back_up_steer)
self._brakeForce = 0.
duration = self._params['back_up'].get('duration', 3.0)
self._update(dt=duration)
self._des_vel = 0.
self._steering = 0.
self._update(dt=duration)
self._brakeForce = 0.
def _is_contact(self):
result = self._world.contactTest(self._vehicle_node)
return result.getNumContacts() > 0
# Environment functions
def reset(self, pos=None, hpr=None, hard_reset=False):
if self._do_back_up and not hard_reset and \
pos is None and hpr is None:
if self._collision:
self._back_up()
else:
if hard_reset:
logger.debug('Hard resetting!')
if pos is None and hpr is None:
pos, hpr = self._next_restart_pos_hpr()
self._place_vehicle(pos=pos, hpr=hpr)
self._collision = False
self._env_time_step = 0
return self._get_observation(), self._get_goal()
def step(self, action):
self._steering = action[0]
if action[1] == 0.0:
self._brakeForce = 1000.
else:
self._brakeForce = 0.
if self._use_vel:
# Convert from m/s to km/h
self._des_vel = action[1]
else:
self._engineForce = self._mass * action[1]
self._update(dt=self._dt)
observation = self._get_observation()
goal = self._get_goal()
reward = self._get_reward()
done = self._get_done()
info = self._get_info()
self._env_time_step += 1
return observation, goal, reward, done, info
class CarEnv(DirectObject):
def __init__(self, params={}):
self._params = params
if 'random_seed' in self._params:
np.random.seed(self._params['random_seed'])
self._use_vel = self._params.get('use_vel', True)
self._run_as_task = self._params.get('run_as_task', False)
self._do_back_up = self._params.get('do_back_up', False)
self._use_depth = self._params.get('use_depth', False)
self._use_back_cam = self._params.get('use_back_cam', False)
self._collision_reward = self._params.get('collision_reward', 0.)
if not self._params.get('visualize', False):
loadPrcFileData('', 'window-type offscreen')
# Defines base, render, loader
try:
ShowBase()
except:
pass
base.setBackgroundColor(0.0, 0.0, 0.0, 1)
# World
self._worldNP = render.attachNewNode('World')
self._world = BulletWorld()
self._world.setGravity(Vec3(0, 0, -9.81))
self._dt = params.get('dt', 0.25)
self._step = 0.05
# Vehicle
shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25))
ts = TransformState.makePos(Point3(0., 0., 0.25))
self._vehicle_node = BulletRigidBodyNode('Vehicle')
self._vehicle_node.addShape(shape, ts)
self._mass = self._params.get('mass', 10.)
self._vehicle_node.setMass(self._mass)
self._vehicle_node.setDeactivationEnabled(False)
self._vehicle_node.setCcdSweptSphereRadius(1.0)
self._vehicle_node.setCcdMotionThreshold(1e-7)
self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node)
self._world.attachRigidBody(self._vehicle_node)
self._vehicle = BulletVehicle(self._world, self._vehicle_node)
self._vehicle.setCoordinateSystem(ZUp)
self._world.attachVehicle(self._vehicle)
self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07)
self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07)
# Camera
size = self._params.get('size', [160, 90])
hfov = self._params.get('hfov', 60)
near_far = self._params.get('near_far', [0.1, 100.])
self._camera_sensor = Panda3dCameraSensor(base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='front cam')
self._camera_node = self._camera_sensor.cam
self._camera_node.setPos(0.0, 0.5, 0.375)
self._camera_node.lookAt(0.0, 6.0, 0.0)
self._camera_node.reparentTo(self._vehicle_pointer)
if self._use_back_cam:
self._back_camera_sensor = Panda3dCameraSensor(
base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='back cam')
self._back_camera_node = self._back_camera_sensor.cam
self._back_camera_node.setPos(0.0, -0.5, 0.375)
self._back_camera_node.lookAt(0.0, -6.0, 0.0)
self._back_camera_node.reparentTo(self._vehicle_pointer)
# Car Simulator
self._des_vel = None
self._setup()
# Input
self.accept('escape', self._doExit)
self.accept('r', self.reset)
self.accept('f1', self._toggleWireframe)
self.accept('f2', self._toggleTexture)
self.accept('f3', self._view_image)
self.accept('f5', self._doScreenshot)
self.accept('q', self._forward_0)
self.accept('w', self._forward_1)
self.accept('e', self._forward_2)
self.accept('a', self._left)
self.accept('s', self._stop)
self.accept('x', self._backward)
self.accept('d', self._right)
self.accept('m', self._mark)
self._steering = 0.0 # degree
self._engineForce = 0.0
self._brakeForce = 0.0
self._p = self._params.get('p', 1.25)
self._d = self._params.get('d', 0.0)
self._last_err = 0.0
self._curr_time = 0.0
self._accelClamp = self._params.get('accelClamp', 2.0)
self._engineClamp = self._accelClamp * self._mass
self._collision = False
if self._run_as_task:
self._mark_d = 0.0
taskMgr.add(self._update_task, 'updateWorld')
base.run()
# _____HANDLER_____
def _doExit(self):
sys.exit(1)
def _toggleWireframe(self):
base.toggleWireframe()
def _toggleTexture(self):
base.toggleTexture()
def _doScreenshot(self):
base.screenshot('Bullet')
def _forward_0(self):
self._des_vel = 1
self._brakeForce = 0.0
def _forward_1(self):
self._des_vel = 2
self._brakeForce = 0.0
def _forward_2(self):
self._des_vel = 4
self._brakeForce = 0.0
def _stop(self):
self._des_vel = 0.0
self._brakeForce = 0.0
def _backward(self):
self._des_vel = -4
self._brakeForce = 0.0
def _right(self):
self._steering = np.min([np.max([-30, self._steering - 5]), 0.0])
def _left(self):
self._steering = np.max([np.min([30, self._steering + 5]), 0.0])
def _view_image(self):
from matplotlib import pyplot as plt
image = self._camera_sensor.observe()[0]
if self._use_depth:
plt.imshow(image[:, :, 0], cmap='gray')
else:
import cv2
def rgb2gray(rgb):
return np.dot(rgb[..., :3], [0.299, 0.587, 0.114])
image = rgb2gray(image)
im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA
) # TODO how does this deal with aspect ratio
plt.imshow(im.astype(np.uint8), cmap='Greys_r')
plt.show()
def _mark(self):
self._mark_d = 0.0
# Setup
def _setup(self):
if hasattr(self, '_model_path'):
# Collidable objects
visNP = loader.loadModel(self._model_path)
visNP.clearModelNodes()
visNP.reparentTo(render)
pos = (0., 0., 0.)
visNP.setPos(pos[0], pos[1], pos[2])
bodyNPs = BulletHelper.fromCollisionSolids(visNP, True)
for bodyNP in bodyNPs:
bodyNP.reparentTo(render)
bodyNP.setPos(pos[0], pos[1], pos[2])
if isinstance(bodyNP.node(), BulletRigidBodyNode):
bodyNP.node().setMass(0.0)
bodyNP.node().setKinematic(True)
bodyNP.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(bodyNP.node())
else:
ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
ground.node().addShape(shape)
ground.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(ground.node())
self._place_vehicle()
self._setup_light()
self._setup_restart_pos()
def _setup_restart_pos(self):
self._restart_pos = []
self._restart_index = 0
if self._params.get('position_ranges', None) is not None:
ranges = self._params['position_ranges']
num_pos = self._params['num_pos']
if self._params.get('range_type', 'random') == 'random':
for _ in range(num_pos):
ran = ranges[np.random.randint(len(ranges))]
self._restart_pos.append(np.random.uniform(ran[0], ran[1]))
elif self._params['range_type'] == 'fix_spacing':
num_ran = len(ranges)
num_per_ran = num_pos // num_ran
for i in range(num_ran):
ran = ranges[i]
low = np.array(ran[0])
diff = np.array(ran[1]) - np.array(ran[0])
for j in range(num_per_ran):
val = diff * ((j + 0.0) / num_per_ran) + low
self._restart_pos.append(val)
elif self._params.get('positions', None) is not None:
self._restart_pos = self._params['positions']
else:
self._restart_pos = self._default_restart_pos()
def _next_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
pos_hpr = self._restart_pos[self._restart_index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _next_random_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
index = np.random.randint(num)
pos_hpr = self._restart_pos[index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _setup_light(self):
alight = AmbientLight('ambientLight')
alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
alightNP = render.attachNewNode(alight)
render.clearLight()
render.setLight(alightNP)
# Vehicle
def _default_pos(self):
return (0.0, 0.0, 0.3)
def _default_hpr(self):
return (0.0, 0.0, 3.14)
def _default_restart_pos():
return [self._default_pos() + self._default_hpr()]
def _get_speed(self):
vel = self._vehicle.getCurrentSpeedKmHour() / 3.6
return vel
def _update(self, dt=1.0, coll_check=True):
self._vehicle.setSteeringValue(self._steering, 0)
self._vehicle.setSteeringValue(self._steering, 1)
self._vehicle.setBrake(self._brakeForce, 0)
self._vehicle.setBrake(self._brakeForce, 1)
self._vehicle.setBrake(self._brakeForce, 2)
self._vehicle.setBrake(self._brakeForce, 3)
if dt >= self._step:
# TODO maybe change number of timesteps
for i in range(int(dt / self._step)):
if self._des_vel is not None:
vel = self._get_speed()
err = self._des_vel - vel
d_err = (err - self._last_err) / self._step
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(self._step, 1, self._step)
self._collision = self._is_contact()
elif self._run_as_task:
self._curr_time += dt
if self._curr_time > 0.05:
if self._des_vel is not None:
vel = self._get_speed()
self._mark_d += vel * self._curr_time
print(vel, self._mark_d, self._is_contact())
err = self._des_vel - vel
d_err = (err - self._last_err) / 0.05
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._curr_time = 0.0
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(dt, 1, dt)
self._collision = self._is_contact()
else:
raise ValueError(
"dt {0} s is too small for velocity control".format(dt))
def _stop_car(self):
self._steering = 0.0
self._engineForce = 0.0
self._vehicle.setSteeringValue(0.0, 0)
self._vehicle.setSteeringValue(0.0, 1)
self._vehicle.applyEngineForce(0.0, 0)
self._vehicle.applyEngineForce(0.0, 1)
self._vehicle.applyEngineForce(0.0, 2)
self._vehicle.applyEngineForce(0.0, 3)
if self._des_vel is not None:
self._des_vel = 0
self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0))
self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0))
for i in range(self._vehicle.getNumWheels()):
wheel = self._vehicle.getWheel(i)
wheel.setRotation(0.0)
self._vehicle_node.clearForces()
def _place_vehicle(self, pos=None, hpr=None):
if pos is None:
pos = self._default_pos()
if hpr is None:
hpr = self._default_hpr()
self._vehicle_pointer.setPos(pos[0], pos[1], pos[2])
self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2])
self._stop_car()
def _addWheel(self, pos, front, radius=0.25):
wheel = self._vehicle.createWheel()
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(1e2)
wheel.setRollInfluence(0.1)
# Task
def _update_task(self, task):
dt = globalClock.getDt()
self._update(dt=dt)
self._get_observation()
return task.cont
# Helper functions
def _get_observation(self):
self._obs = self._camera_sensor.observe()
observation = []
observation.append(self._obs[0])
if self._use_back_cam:
self._back_obs = self._back_camera_sensor.observe()
observation.append(self._back_obs[0])
observation = np.concatenate(observation, axis=2)
return observation
def _get_reward(self):
reward = self._collision_reward if self._collision else self._get_speed(
)
return reward
def _get_done(self):
return self._collision
def _get_info(self):
info = {}
info['pos'] = np.array(self._vehicle_pointer.getPos())
info['hpr'] = np.array(self._vehicle_pointer.getHpr())
info['vel'] = self._get_speed()
info['coll'] = self._collision
return info
def _back_up(self):
assert (self._use_vel)
back_up_vel = self._params['back_up'].get('vel', -2.0)
self._des_vel = back_up_vel
back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0))
# TODO
self._steering = np.random.uniform(*back_up_steer)
self._brakeForce = 0.
duration = self._params['back_up'].get('duration', 1.0)
self._update(dt=duration)
self._des_vel = 0.0
self._steering = 0.0
self._update(dt=duration)
self._brakeForce = 0.
def _is_contact(self):
result = self._world.contactTest(self._vehicle_node)
num_contacts = result.getNumContacts()
return result.getNumContacts() > 0
# Environment functions
def reset(self, pos=None, hpr=None, hard_reset=False, random_reset=False):
if self._do_back_up and not hard_reset and \
pos is None and hpr is None:
if self._collision:
self._back_up()
else:
if pos is None and hpr is None:
if random_reset:
pos, hpr = self._next_random_restart_pos_hpr()
else:
pos, hpr = self._next_restart_pos_hpr()
self._place_vehicle(pos=pos, hpr=hpr)
self._collision = False
return self._get_observation()
def step(self, action):
self._steering = action[0]
if action[1] == 0.0:
self._brakeForce = 1000.
else:
self._brakeForce = 0.
if self._use_vel:
# Convert from m/s to km/h
self._des_vel = action[1]
else:
self._engineForce = self._engineClamp * \
((action[1] - 49.5) / 49.5)
self._update(dt=self._dt)
observation = self._get_observation()
reward = self._get_reward()
done = self._get_done()
info = self._get_info()
return observation, reward, done, info
class Drone:
# RIGIDBODYMASS = 1
# RIGIDBODYRADIUS = 0.1
# LINEARDAMPING = 0.97
# SENSORRANGE = 0.6
# TARGETFORCE = 3
# AVOIDANCEFORCE = 25
# FORCEFALLOFFDISTANCE = .5
RIGIDBODYMASS = 0.5
RIGIDBODYRADIUS = 0.1
LINEARDAMPING = 0.95
SENSORRANGE = 0.6
TARGETFORCE = 1
AVOIDANCEFORCE = 10
FORCEFALLOFFDISTANCE = .5
def __init__(self,
manager,
position: Vec3,
uri="-1",
printDebugInfo=False):
self.base = manager.base
self.manager = manager
# The position of the real drone this virtual drone is connected to.
# If a connection is active, this value is updated each frame.
self.realDronePosition = Vec3(0, 0, 0)
self.canConnect = False # true if the virtual drone has a uri to connect to a real drone
self.isConnected = False # true if the connection to a real drone is currently active
self.uri = uri
if self.uri != "-1":
self.canConnect = True
self.id = int(uri[-1])
self.randVec = Vec3(random.uniform(-1, 1), random.uniform(-1, 1),
random.uniform(-1, 1))
# add the rigidbody to the drone, which has a mass and linear damping
self.rigidBody = BulletRigidBodyNode(
"RigidSphere") # derived from PandaNode
self.rigidBody.setMass(self.RIGIDBODYMASS) # body is now dynamic
self.rigidBody.addShape(BulletSphereShape(self.RIGIDBODYRADIUS))
self.rigidBody.setLinearSleepThreshold(0)
self.rigidBody.setFriction(0)
self.rigidBody.setLinearDamping(self.LINEARDAMPING)
self.rigidBodyNP = self.base.render.attachNewNode(self.rigidBody)
self.rigidBodyNP.setPos(position)
# self.rigidBodyNP.setCollideMask(BitMask32.bit(1))
self.base.world.attach(self.rigidBody)
# add a 3d model to the drone to be able to see it in the 3d scene
model = self.base.loader.loadModel(self.base.modelDir +
"/drones/drone1.egg")
model.setScale(0.2)
model.reparentTo(self.rigidBodyNP)
self.target = position # the long term target that the virtual drones tries to reach
self.setpoint = position # the immediate target (setpoint) that the real drone tries to reach, usually updated each frame
self.waitingPosition = Vec3(position[0], position[1], 0.7)
self.lastSentPosition = self.waitingPosition # the position that this drone last sent around
self.printDebugInfo = printDebugInfo
if self.printDebugInfo: # put a second drone model on top of drone that outputs debug stuff
model = self.base.loader.loadModel(self.base.modelDir +
"/drones/drone1.egg")
model.setScale(0.4)
model.setPos(0, 0, .2)
model.reparentTo(self.rigidBodyNP)
# initialize line renderers
self.targetLineNP = self.base.render.attachNewNode(LineSegs().create())
self.velocityLineNP = self.base.render.attachNewNode(
LineSegs().create())
self.forceLineNP = self.base.render.attachNewNode(LineSegs().create())
self.actualDroneLineNP = self.base.render.attachNewNode(
LineSegs().create())
self.setpointNP = self.base.render.attachNewNode(LineSegs().create())
def connect(self):
"""Connects the virtual drone to a real one with the uri supplied at initialization."""
if not self.canConnect:
return
print(self.uri, "connecting")
self.isConnected = True
self.scf = SyncCrazyflie(self.uri, cf=Crazyflie(rw_cache='./cache'))
self.scf.open_link()
self._reset_estimator()
self.start_position_printing()
# MOVE THIS BACK TO SENDPOSITIONS() IF STUFF BREAKS
self.scf.cf.param.set_value('flightmode.posSet', '1')
def sendPosition(self):
"""Sends the position of the virtual drone to the real one."""
cf = self.scf.cf
self.setpoint = self.getPos()
# send the setpoint
cf.commander.send_position_setpoint(self.setpoint[0], self.setpoint[1],
self.setpoint[2], 0)
def disconnect(self):
"""Disconnects the real drone."""
print(self.uri, "disconnecting")
self.isConnected = False
cf = self.scf.cf
cf.commander.send_stop_setpoint()
time.sleep(0.1)
self.scf.close_link()
def update(self):
"""Update the virtual drone."""
# self.updateSentPositionBypass(0)
self._updateTargetForce()
self._updateAvoidanceForce()
self._clampForce()
if self.isConnected:
self.sendPosition()
# draw various lines to get a better idea of whats happening
self._drawTargetLine()
# self._drawVelocityLine()
self._drawForceLine()
# self._drawSetpointLine()
self._printDebugInfo()
def updateSentPosition(self, timeslot):
transmissionProbability = 1
if self.id == timeslot:
if random.uniform(0, 1) < transmissionProbability:
# print(f"drone {self.id} updated position")
self.lastSentPosition = self.getPos()
else:
pass
# print(f"drone {self.id} failed!")
def updateSentPositionBypass(self, timeslot):
self.lastSentPosition = self.getPos()
def getPos(self) -> Vec3:
return self.rigidBodyNP.getPos()
def getLastSentPos(self) -> Vec3:
return self.lastSentPosition
def _updateTargetForce(self):
"""Applies a force to the virtual drone which moves it closer to its target."""
dist = (self.target - self.getPos())
if (dist.length() > self.FORCEFALLOFFDISTANCE):
force = dist.normalized()
else:
force = (dist / self.FORCEFALLOFFDISTANCE)
self.addForce(force * self.TARGETFORCE)
def _updateAvoidanceForce(self):
"""Applies a force the the virtual drone which makes it avoid other drones."""
# get all drones within the sensors reach and put them in a list
nearbyDrones = []
for drone in self.manager.drones:
dist = (drone.getLastSentPos() - self.getPos()).length()
if drone.id == self.id: # prevent drone from detecting itself
continue
if dist < self.SENSORRANGE:
nearbyDrones.append(drone)
# calculate and apply forces
for nearbyDrone in nearbyDrones:
distVec = nearbyDrone.getLastSentPos() - self.getPos()
if distVec.length() < 0.2:
print("BONK")
distMult = self.SENSORRANGE - distVec.length()
avoidanceDirection = self.randVec.normalized(
) * 2 - distVec.normalized() * 10
avoidanceDirection.normalize()
self.addForce(avoidanceDirection * distMult * self.AVOIDANCEFORCE)
def _clampForce(self):
"""Clamps the total force acting in the drone."""
totalForce = self.rigidBody.getTotalForce()
if totalForce.length() > 2:
self.rigidBody.clearForces()
self.rigidBody.applyCentralForce(totalForce.normalized())
def targetVector(self) -> Vec3:
"""Returns the vector from the drone to its target."""
return self.target - self.getPos()
def _printDebugInfo(self):
if self.printDebugInfo:
pass
def setTarget(self, target: Vec3):
"""Sets a new target for the drone."""
self.target = target
def setRandomTarget(self):
"""Sets a new random target for the drone."""
self.target = self.manager.getRandomRoomCoordinate()
def setNewRandVec(self):
self.randVec = Vec3(random.uniform(-1, 1), random.uniform(-1, 1),
random.uniform(-1, 1))
def addForce(self, force: Vec3):
self.rigidBody.applyCentralForce(force)
def setPos(self, position: Vec3):
self.rigidBodyNP.setPos(position)
def getVel(self) -> Vec3:
return self.rigidBody.getLinearVelocity()
def setVel(self, velocity: Vec3):
return self.rigidBody.setLinearVelocity(velocity)
def _drawTargetLine(self):
self.targetLineNP.removeNode()
ls = LineSegs()
# ls.setThickness(1)
ls.setColor(1.0, 0.0, 0.0, 1.0)
ls.moveTo(self.getPos())
ls.drawTo(self.target)
node = ls.create()
self.targetLineNP = self.base.render.attachNewNode(node)
def _drawVelocityLine(self):
self.velocityLineNP.removeNode()
ls = LineSegs()
# ls.setThickness(1)
ls.setColor(0.0, 0.0, 1.0, 1.0)
ls.moveTo(self.getPos())
ls.drawTo(self.getPos() + self.getVel())
node = ls.create()
self.velocityLineNP = self.base.render.attachNewNode(node)
def _drawForceLine(self):
self.forceLineNP.removeNode()
ls = LineSegs()
# ls.setThickness(1)
ls.setColor(0.0, 1.0, 0.0, 1.0)
ls.moveTo(self.getPos())
ls.drawTo(self.getPos() + self.rigidBody.getTotalForce() * 0.2)
node = ls.create()
self.forceLineNP = self.base.render.attachNewNode(node)
def _drawSetpointLine(self):
self.setpointNP.removeNode()
ls = LineSegs()
# ls.setThickness(1)
ls.setColor(1.0, 1.0, 1.0, 1.0)
ls.moveTo(self.getPos())
ls.drawTo(self.setpoint)
node = ls.create()
self.setpointNP = self.base.render.attachNewNode(node)
def _wait_for_position_estimator(self):
"""Waits until the position estimator reports a consistent location after resetting."""
print('Waiting for estimator to find position...')
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
log_config.add_variable('kalman.varPX', 'float')
log_config.add_variable('kalman.varPY', 'float')
log_config.add_variable('kalman.varPZ', 'float')
var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10
threshold = 0.001
with SyncLogger(self.scf, log_config) as logger:
for log_entry in logger:
data = log_entry[1]
var_x_history.append(data['kalman.varPX'])
var_x_history.pop(0)
var_y_history.append(data['kalman.varPY'])
var_y_history.pop(0)
var_z_history.append(data['kalman.varPZ'])
var_z_history.pop(0)
min_x = min(var_x_history)
max_x = max(var_x_history)
min_y = min(var_y_history)
max_y = max(var_y_history)
min_z = min(var_z_history)
max_z = max(var_z_history)
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (max_z -
min_z) < threshold:
break
def _reset_estimator(self):
"""Resets the position estimator, this should be run before flying the drones or they might report a wrong position."""
cf = self.scf.cf
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
self._wait_for_position_estimator()
def position_callback(self, timestamp, data, logconf):
"""Updates the variable holding the position of the actual drone. It is not called in the update method, but by the drone itself (I think)."""
x = data['kalman.stateX']
y = data['kalman.stateY']
z = data['kalman.stateZ']
self.realDronePosition = Vec3(x, y, z)
# print('pos: ({}, {}, {})'.format(x, y, z))
def start_position_printing(self):
"""Activate logging of the position of the real drone."""
log_conf = LogConfig(name='Position', period_in_ms=50)
log_conf.add_variable('kalman.stateX', 'float')
log_conf.add_variable('kalman.stateY', 'float')
log_conf.add_variable('kalman.stateZ', 'float')
self.scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.position_callback)
log_conf.start()
