def create_hinge_scene():
size = [0.5, 0.25, 1]
b1, b2 = create_bodies([0, 0, 0], [0, 0, -2.5], size)
demoutils.sim().add(b1)
demoutils.sim().add(b2)
f1 = agx.Frame()
f1.setLocalTranslate(0, 0, size[2])
f1.setLocalRotate(agx.EulerAngles(0, math.radians(90), 0))
hinge1 = agx.Hinge(b1, f1)
demoutils.sim().add(hinge1)
# Make the first motor swing back and forth
speed_controller = AlternatingSpeedController(hinge1.getMotor1D(), 1, 2)
demoutils.sim().add(speed_controller)
distance = (b2.getPosition() - b1.getPosition()).length()
f1 = agx.Frame()
f1.setLocalTranslate(0, 0, -distance / 2)
f1.setLocalRotate(agx.EulerAngles(0, math.radians(90), 0))
f2 = agx.Frame()
f2.setLocalTranslate(0, 0, distance / 2)
f2.setLocalRotate(agx.EulerAngles(0, math.radians(90), 0))
hinge2 = agx.Hinge(b1, f1, b2, f2)
demoutils.sim().add(hinge2)
def build_scene3():
create_and_add_ground()
servo = create_and_add_servo()
servo.setPosition(0, 0, 0.085)
servo.setRotation(agx.EulerAngles(0, -math.pi / 2, 0))
controller = SimpleServoPositionController(servo)
demoutils.sim().addEventListener(controller)
def build_scene():
floor = agxCollide.Geometry(agxCollide.Box(10, 10, 0.1))
floor.setPosition(0, 0, -4)
demoutils.sim().add(floor)
# Create each and every one of the scenes
create_hinge_scene()
demoutils.app().getSceneDecorator().setEnableShadows(False)
demoutils.app().setEnableDebugRenderer(True)
def add_objects_to_sim(*args):
"""
adds all objects provided to the simulation.
Args:
*args:
Returns:
"""
for obj in args:
sim().add(obj)
def post(self, t):
pos, n = self.getPosition()
if pos[0] - WATER_LENGTH * n > 50:
print("build more water")
self.n += 1
sea, floor = make_water.MakeWater.make_water(WATER_DENSITY, WATER_LENGTH, WATER_WIDTH, WATER_DEPTH)
p = sea.getPosition()
sea.setPosition(p[0] + WATER_LENGTH * self.n, p[1], p[2])
p = floor.getPosition()
floor.setPosition(p[0] + WATER_LENGTH * self.n, p[1], p[2])
self.controller.addWater(sea)
demoutils.sim().add(sea)
demoutils.sim().add(floor)
def build_scene1():
# Create a geometry with a plane shape as our 'floor'
floor_geometry = agxCollide.Geometry(agxCollide.Box(agx.Vec3(10, 10, 0.1)))
demoutils.create_visual(floor_geometry)
demoutils.sim().add(floor_geometry) # Add the geometry to the simulation
rb1 = agx.RigidBody() # Create a rigid body
rb1.add(agxCollide.Geometry(agxCollide.Sphere(
0.5))) # Add a geometry with a sphere-shape of radius 0.5
rb1.setPosition(0, 0, 5.0) # Position the sphere somewhere above our plane
demoutils.create_visual(rb1)
demoutils.sim().add(
rb1) # Add the body to the simulation. The geometry will also be added
def build_scene2():
# Create a geometry with a plane shape as our 'floor'
floor_geometry = agxCollide.Geometry(agxCollide.Box(agx.Vec3(10, 10, 0.1)))
demoutils.create_visual(floor_geometry, diffuse_color=Color.Green())
demoutils.sim().add(floor_geometry) # Add the geometry to the simulation
for x in range(-5, 5):
for y in range(-5, 5):
for z in range(1, 8):
rb = agx.RigidBody() # Create a rigid body
rb.add(agxCollide.Geometry(agxCollide.Sphere(
0.2))) # Add a geometry with a sphere-shape of radius 0.2
rb.setPosition(
x + random(), y + random(),
z) # Position the sphere somewhere above our plane
demoutils.create_visual(rb)
demoutils.sim().add(
rb
) # Add the body to the simulation. The geometry will also be added
def build_scene():
crane = CraneAssembly()
demoutils.sim().add(crane)
demoutils.sim().addEventListener(CraneController(crane))
lock = agx.LockJoint(crane.link1)
demoutils.sim().add(lock)
def build_scene():
sim = demoutils.sim()
# Create a geometry with a plane shape as our 'floor'
floor_geometry = agxCollide.Geometry(agxCollide.Box(agx.Vec3(10, 10, 0.1)))
demoutils.create_visual(floor_geometry, Color.Green())
sim.add(floor_geometry) # Add the geometry to the simulation
sim.add(create_box(agx.Vec3(-2, 3, 5)))
sim.add(create_sphere(agx.Vec3(0, 1, 5)))
sim.add(create_capsule(agx.Vec3(3, 5, 5)))
sim.add(create_cylinder(agx.Vec3(-3, -2, 5)))
def display_forces(self, t):
tot_contact_force = agx.Vec3()
contacts = demoutils.sim().getSpace().getGeometryContacts()
for contact in contacts:
points = contact.points()
for p in points:
f = p.getForce()
tot_contact_force = tot_contact_force + f
demoutils.app().getSceneDecorator().setText(0, "Towed-Rov simulation")
demoutils.app().getSceneDecorator().setText(1, "Thrust : {} kN".format(
self.m_propulsion_force / 500)) # 2/1000 ( 2 because of the 2 propellers )
demoutils.app().getSceneDecorator().setText(2, "Speed in X direction : {} knots".format(
str(round(self.m_body.getVelocity()[0] * 1.94384449, 2))))
def init(self, width, length, rFender):
ship = agx.RigidBody()
self.add(ship)
self.m_body = ship
self.m_body.setName('boat')
half_length = length * 0.5
half_width = width * 0.5
half_height = 0.25 * half_width
b = agxCollide.Geometry(agxCollide.Box(half_length, half_width, half_height))
b.setName('ship')
"""Capsules"""
radius = half_height * 1.2
left_c = agxCollide.Geometry(agxCollide.Capsule(radius, length - 2 * radius))
left_c.setRotation(agx.Quat(math.pi * 0.5, agx.Vec3.Z_AXIS()))
left_c.setPosition(0, half_width - radius, - (half_height + radius))
right_capsules = agxCollide.Geometry(agxCollide.Capsule(radius, length - 2 * radius))
right_capsules.setRotation(agx.Quat(math.pi * 0.5, agx.Vec3.Z_AXIS()))
right_capsules.setPosition(0, radius - half_width, - (half_height + radius))
"""Fender"""
fender_material = agx.Material("fenderMaterial")
landing_material = agx.Material("landingMaterial")
contact_material = demoutils.sim().getMaterialManager().getOrCreateContactMaterial(fender_material,
landing_material)
contact_material.setYoungsModulus(5e5)
contact_material.setFrictionCoefficient(1.0)
contact_material.setDamping(0.4)
self.create_fenders(fender_material, rFender, half_width, half_height, half_length)
"""Top"""
t_box = agxCollide.Geometry(agxCollide.Box(half_length * 0.5, half_width * 0.5, half_height))
t_box.setPosition(-0.4, 0, 2 * half_height)
tt_box = agxCollide.Geometry(agxCollide.Box(half_length * 0.2, half_width * 0.4, half_height * 1.1))
tt_box.setPosition(0, 0, 4.1 * half_height)
"""Assemble ship"""
ship.add(b) # base
ship.add(left_c) # left capsule
ship.add(right_capsules) # left fender
ship.add(t_box) # box on top of base
ship.add(tt_box) # box on top of box on top of base
ship.setPosition(-90, 0, 0)
self.n = 1
self.m_left_propeller = agx.Vec3(-half_length, half_width - radius, - (half_height + 2 * radius))
self.m_right_propeller = agx.Vec3(-half_length, radius - half_width, - (half_height + 2 * radius))
def build_scene():
listener = MyStepEventListener()
demoutils.sim().addEventListener(listener)
def create_and_add_servo():
servo = ServoAssembly()
demoutils.sim().add(servo)
return servo
def create_and_add_ground():
ground = agxCollide.Geometry(agxCollide.Box(1, 1, 0.05))
demoutils.create_visual(ground, Color.Green())
demoutils.sim().add(ground)
return ground
def build_scene():
"""
builds the simulation, is run by agx internaly.
Returns:
"""
print("build staring")
""" Ads Water and seafloor to the simulation"""
water_geometry, seafloor = MakeWater.make_water(WATER_DENSITY,
WATER_LENGTH, WATER_WIDTH,
WATER_DEPTH)
controller = agxModel.WindAndWaterController()
controller.addWater(water_geometry)
print("buildt water controller")
"""Creates a pid controller for depth and trim"""
rov_pid = build_pid_controller(p=ROV_K_P,
i=ROV_K_I,
d=ROV_K_D,
mode=(1, 0, 0),
setpoint=ROV_DEPTH_SETPOINT,
direction=1,
max_out=MAX_WING_ANGLE,
min_out=MIN_WING_ANGLE,
name="rov_pid")
pid_trim = build_pid_controller(p=K_P_TRIM,
i=K_I_TRIM,
d=K_D_TRIM,
direction=0,
mode=(1, 0, 0),
max_out=TRIM_MAX_OUT,
min_out=TRIM_MIN_OUT,
name="pidTrim",
setpoint=0)
keyboard = KeyboardListener(rov_pid, plot)
print("buildt pid's for rov")
"""Creates the rov"""
rov = RovAssembly(keyboard)
rov.setPosition(agx.Vec3(-WATER_LENGTH + 10, 0, 0))
rov.setName("rov")
rov.setRotation(agx.EulerAngles(0, 0, math.pi))
setBodyViscousDrag(rov, controller)
print("buildt rov")
"""builds a lock for the system to stablilize it the lock is soft so that the rov can move but it takes more
to move it"""
lock = build_angular_lockJoint(rov.getRigidBody('rovBody'), 1e-2, 1e-12,
1e-2, 1e-12)
print("buildt lock for rov")
""" creates arduino simulations"""
arduino_sensor = ArduinoSensor(rov, seafloor.getShape().asHeightField())
arduino_stepper = ArduinoStepper(rov_pid, pid_trim, rov)
print("buildt pid for boat")
pid_boat = build_pid_controller(p=BOAT_K_P,
i=BOAT_K_I,
d=BOAT_K_D,
direction=0,
name="pidBoat",
max_out=2,
min_out=-2,
setpoint=BOAT_SPEED,
mode=(1, 0, 0))
"""Creates the boat to tow the rov"""
ship = Ship(controller)
ship.setName('ship')
ship.setRotation(agx.EulerAngles(0, 0, math.pi))
ship.setPosition(agx.Vec3(-WATER_LENGTH + 20 + WIRE_LENGTH, 0, 0))
ship_echo = Boat_Sensor(ship, seafloor.getShape().asHeightField())
ship.setVelocity(agx.Vec3(-1, 0, 0))
ship_controller = Boat_Controller(ship, pid_boat, arduino_stepper)
print("buildt ship")
"""builds the wire and wire controller"""
wire, wire_renderer = MakeWire().create_wire(1030, 0.001, ship,
agx.Vec3(2, 0, 0), rov,
agx.Vec3(*WIRE_POS_ROV))
setWireViscousDrag(wire, controller)
print("build Wire")
"""Adds rov, boat, controller, and pid to the simulation"""
add_objects_to_sim(arduino_sensor, arduino_stepper, wire, wire_renderer)
print("added sensor, stepper, wire and wire rederer")
add_objects_to_sim(ship, ship_controller, seafloor, water_geometry,
controller)
print(
"added ship and ship controller, seafloor, water geometry and controller"
)
add_objects_to_sim(rov, rov_pid, lock)
print("added rov and rov_pid and lock")
sim().addEventListener(ship_echo)
print("added ship sensor")
sim().addEventListener(keyboard)
print("added keyboard listener")
sim().setTimeStep(SIM_TIME_STEP)
set_camera_to_rov(rov)
print_builancy(rov.link1)
"""locks the rov in fixed position, for mounting wing and cable to rov"""
if adjust_rov:
lock1 = agx.LockJoint(rov.link1)
sim().add(lock1)
