def __init__(self, material: agx.Material = None):
super().__init__()
visual_geometry = agxCollide.Geometry(intermediate_shape.deepCopy(),
agx.AffineMatrix4x4.rotate(math.pi / 2, 0, 1, 0) *
agx.AffineMatrix4x4.rotate(math.pi, 1, 0, 0))
visual_geometry.setEnableCollisions(False)
snakeapp.create_visual(visual_geometry, agxRender.Color.Orange())
collision_geometry = agxCollide.Geometry(agxCollide.Box(intermediate_bounds),
agx.AffineMatrix4x4.translate(intermediate_len / 2, 0, 0))
self.body = agx.RigidBody()
self.body.add(visual_geometry)
self.body.add(collision_geometry)
self.add(self.body)
sensor_geometry = agxCollide.Geometry(agxCollide.Box(sensor_bounds),
agx.AffineMatrix4x4.translate(intermediate_len / 2, -0.035, 0))
self.sensor = agx.RigidBody(sensor_geometry)
self.add(self.sensor)
if material is not None:
collision_geometry.setMaterial(material)
sensor_geometry.setMaterial(material)
self.merged_body = agx.MergedBody()
self.merged_body.add(agx.MergedBodyEmptyEdgeInteraction(self.body, self.sensor))
snakeapp.add(self.merged_body)
def __init__(self, material: agx.Material = None):
super().__init__()
servo = agxCollide.Geometry(servo_shape.deepCopy())
servo.setEnableCollisions(False)
snakeapp.create_visual(servo, diffuse_color=agxRender.Color.Black())
self.bottom = agx.RigidBody(
agxCollide.Geometry(bottom_shape.deepCopy()))
self.bottom.add(servo)
snakeapp.create_visual(self.bottom, agxRender.Color.Orange())
self.upper = agx.RigidBody(agxCollide.Geometry(upper_shape.deepCopy()))
snakeapp.create_visual(self.upper, agxRender.Color.Orange())
if material is not None:
self.bottom.getGeometries()[0].setMaterial(material)
self.upper.getGeometries()[0].setMaterial(material)
self.hinge = create_constraint(pos=agx.Vec3(0.0, 0.0007, 0),
axis=agx.agx.Vec3(0, 0, -1),
rb1=self.bottom,
rb2=self.upper,
c=agx.Hinge) # type: agx.Hinge
self.hinge.setCompliance(1E-12)
self.hinge.getMotor1D().setCompliance(1E-10)
self.hinge.getMotor1D().setEnable(True)
self.hinge.getLock1D().setEnable(False)
self.hinge.getRange1D().setRange(-math.pi / 2, math.pi / 2)
self.add(self.bottom)
self.add(self.hinge)
self.add(self.upper)
def __init__(self, material):
props = BrickProps()
props.length *= 2
props.mass *= 3
super(Duplo4x2Cone, self).__init__(props)
self.add_top(props, material)
self.add_walls(props, material)
self.add_center(props, material)
self.add_center(props, material, x=props.stud_centerdist * 2)
self.add_center(props, material, x=-props.stud_centerdist * 2)
self.add_extrudes(props, material)
self.add_stud(props, material, 3 * props.stud_centerdist, props.stud_centerdist)
self.add_stud(props, material, 3 * props.stud_centerdist, -props.stud_centerdist)
self.add_stud(props, material, -3 * props.stud_centerdist, props.stud_centerdist)
self.add_stud(props, material, -3 * props.stud_centerdist, -props.stud_centerdist)
#Add cone
base = agxCollide.Geometry(agxCollide.Box((props.width - 0.001) / 2, (props.width - 0.001) / 2, props.studcyl_height * 1.5))
self.brick_rb.add(base)
base.setLocalPosition(0, 0, props.height + props.studcyl_height * 1.5)
base.setMaterial(material)
cone_topradius = 2 * props.stud_radius
cone_bottomradius = (props.width -0.001) / 2
cone_height = 0.038
cone = agxCollide.Geometry(agxCollide.Cone(cone_topradius, cone_bottomradius, cone_height))
cone.setMaterial(material)
rotate_x(cone, 90)
self.brick_rb.add(cone)
cone.setLocalPosition(0, 0, props.height)
self.add_stud(props, material, x=0, y=0, z=cone_height)
self.height += cone_height #Add cone height
self.brick_rb.setRotation(agx.EulerAngles(0, 0, math.radians(-90)))
def buildArena(arena_pos):
sim = agxPython.getContext().environment.getSimulation()
app = agxPython.getContext().environment.getApplication()
root = agxPython.getContext().environment.getSceneRoot()
arena_size = [width, width, 0.2]
h = 0.35
floor = agx.RigidBody(
agxCollide.Geometry(
agxCollide.Box(arena_size[0] / 2, arena_size[1] / 2,
arena_size[2] / 2)))
floor.setPosition(arena_pos[0], arena_pos[1],
arena_pos[2] - arena_size[2] / 2)
floor.setMotionControl(1)
sim.add(floor)
agxOSG.setDiffuseColor(agxOSG.createVisual(floor, root),
agxRender.Color.Gray())
# Octagon sides
sides = 8
skip_sides = [9]
side_len = width / (1 + np.sqrt(2)) + arena_size[2] / 2 / np.sqrt(2)
base_pos = agx.Vec3(arena_pos[0], arena_pos[1],
arena_pos[2] - arena_size[2] / 2 + h / 2)
for w in range(sides):
if w not in skip_sides:
theta = -w * np.pi / 4
rot = agx.Quat(theta, agx.Vec3(0, 0, 1))
rot_pos = agx.Vec3(
np.sin(theta) * width / 2, -np.cos(theta) * width / 2, 0)
wall = agx.RigidBody(
agxCollide.Geometry(
agxCollide.Box(side_len / 2, arena_size[2] / 2, h / 2)))
wall.setPosition(base_pos + rot_pos)
wall.setMotionControl(1)
wall.setRotation(rot)
sim.add(wall)
agxOSG.setDiffuseColor(agxOSG.createVisual(wall, root),
agxRender.Color.DarkGray())
# Ramp up to the course
ramp_dim = [1.4, side_len, 0.2] # *np.cos(np.pi/4)
ramp = agx.RigidBody(
agxCollide.Geometry(
agxCollide.Box(ramp_dim[0] / 2, ramp_dim[1] / 2, ramp_dim[2] / 2)))
theta = -np.arcsin(ramp_dim[2] / ramp_dim[0]) / 2
ramp.setPosition(
arena_pos[0] - arena_size[0] / 2 - ramp_dim[0] / 2 * np.cos(theta) -
ramp_dim[2] / 2 * np.sin(theta), arena_pos[1],
arena_pos[2] - arena_size[2] * 3 / 4) # +arena_size[1]/2-ramp_dim[1]/2
ramp.setRotation(agx.Quat(theta, agx.Vec3(0, 1, 0)))
ramp.setMotionControl(1)
sim.add(ramp)
agxOSG.setDiffuseColor(agxOSG.createVisual(ramp, root),
agxRender.Color.Gray())
obstacles(sim, root, arena_pos)
def create_bodies(position1, position2, size):
b1 = agx.RigidBody()
b2 = agx.RigidBody()
b1.add(agxCollide.Geometry(agxCollide.Box(size[0], size[1], size[2])))
b2.add(agxCollide.Geometry(agxCollide.Box(size[0], size[1], size[2])))
b1.setPosition(agx.Vec3(position1[0], position1[1], position1[2]))
b2.setPosition(agx.Vec3(position2[0], position2[1], position2[2]))
return b1, b2
def __init__(self):
super().__init__()
servo_geometry = agxCollide.Geometry(servo_shape.deepCopy())
servo_geometry.setEnableCollisions(False)
snakeapp.create_visual(servo_geometry, agxRender.Color.Gray())
bottom_geometry = agxCollide.Geometry(bottom_shape.deepCopy())
bottom_geometry.setEnableCollisions(False)
snakeapp.create_visual(bottom_geometry, agxRender.Color.Black())
self.add(servo_geometry)
self.add(bottom_geometry)
def add_stud(self, props, material, x, y, z=0):
""" Adds stud at (x, y) """
base = agxCollide.Geometry(agxCollide.Cylinder(props.stud_radius, props.studcyl_height))
base.setMaterial(material)
rotate_x(base, 90)
top = agxCollide.Geometry(agxCollide.Cone(props.studtop_inner_rad, props.stud_radius, props.studtop_height))
top.setMaterial(material)
rotate_x(top, 90)
self.brick_rb.add(base)
self.brick_rb.add(top)
base.setLocalPosition(x, y, z + props.height + props.studcyl_height / 2)
top.setLocalPosition(x, y, z + props.height + props.studcyl_height)
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 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 add_center(self, props, material, x=0, y=0, z=0):
""" Add hollow center cylinder """
center = agxCollide.Geometry(agxCollide.HollowCylinder(props.center_radius - props.wall_width, props.center_height, props.wall_width))
center.setMaterial(material)
self.brick_rb.add(center)
rotate_x(center, 90)
center.setLocalPosition(x, y, z + props.wall_height / 2)
def build_geomery(shape: agxCollide.Shape, name: str, material,
rot) -> agxCollide.Geometry:
geom = agxCollide.Geometry(shape)
geom.setMaterial(material)
geom.setName(name)
geom.setRotation(agx.EulerAngles(*rot))
return geom
def setup_scene(i: int):
snake = Snake(NUM_SNAKE_MODULES, pitch_only=False,
with_camera=True) # type: Snake
snake.setPosition(agx.Vec3(0, 0, 0.1))
snakeapp.add(snake)
plane_body = agx.RigidBody(
agxCollide.Geometry(agxCollide.Box(2, 2, 0.1),
agx.AffineMatrix4x4.translate(0, 0, -0.1 / 2)))
plane_body.setMotionControl(agx.RigidBody.STATIC)
snakeapp.create_visual(plane_body, diffuse_color=agxRender.Color.Green())
snakeapp.add(plane_body)
snake_controller = SnakeControl(snake)
if i == FLAPPING:
snake_controller.init_flapping(math.pi / 9.0, math.pi / 9.0, 16.0,
-math.pi * 5.0 / 180.0)
elif i == TURNING:
snake_controller.init_turning(math.pi / 9.0, math.pi * 2.0 / 3.0, 8.0,
0.0, math.pi * 20.0 / 180.0)
elif i == SIDEWINDING:
snake_controller.init_sidewinding(math.pi / 9.0, math.pi * 2.0 / 3.0,
16.0)
elif i == ROLLING:
snake_controller.init_rolling(math.pi / 6.0, math.pi / 6.0, 16.0)
elif i == ROTATING:
snake_controller.init_rotating(math.pi / 6.0, math.pi / 6.0, 16.0)
snakeapp.add_event_listener(snake_controller)
snakeapp.init_camera(eye=agx.Vec3(-1, -1, 0.5),
center=plane_body.getPosition())
def create_bodies(position1, position2, size):
# Create first leg section and set position
leg_section1 = agx.RigidBody()
leg_section1.add(
agxCollide.Geometry(agxCollide.Box(size[0], size[1], size[2])))
leg_section1.setPosition(agx.Vec3(position1[0], position1[1],
position1[2]))
# Create second leg section and set position
leg_section2 = agx.RigidBody()
leg_section2.add(
agxCollide.Geometry(agxCollide.Box(size[0], size[1], size[2])))
leg_section2.setPosition(agx.Vec3(position2[0], position2[1],
position2[2]))
return leg_section1, leg_section2
def create_sphere(position: agx.Vec3):
shape = agxCollide.Sphere(0.5)
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
demoutils.create_visual(body)
return body
def add_stud_sensor(self, props, x, y):
""" Adds sensor at stud for peg in hole simulation """
stud_sensor = agxCollide.Geometry(agxCollide.Sphere(props.stud_radius))
stud_sensor.addGroup("stud")
stud_sensor.setSensor(True)
self.brick_rb.add(stud_sensor)
stud_sensor.setLocalPosition(x, y, props.height + props.studcyl_height - props.stud_radius)
def create_sphere(position: agx.Vec3, scale):
shape = agxCollide.Sphere(0.5 * scale)
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
oneLegRobotApp.create_visual(body)
return body
def make_water(water_density, water_length, water_width, water_height):
"""
Args:
water_density:
water_length:
water_width:
water_height:
Returns:
"""
seafloorImageBuilder.save_new_seafloor_image(width=water_length)
water_material = agx.Material("waterMaterial")
water_material.getBulkMaterial().setDensity(water_density)
water_geometry = agxCollide.Geometry(
agxCollide.Box(water_length, water_width / 2, water_height / 2))
water_geometry.setPosition(0, 0, -water_height / 2)
seafloor = MakeWater.make_seafloor(vairance=SEAFLOOR_VARIANCE,
length=water_length * 2,
width=water_width,
depth=-water_height)
"""Surface of water at z = 0."""
water_geometry.setMaterial(water_material)
MakeWater.add_color(water_geometry)
""" ads visualisation"""
agxOSG.createVisual(seafloor, demoutils.root())
print("build water and seafloor")
return water_geometry, seafloor
def create_box(position: agx.Vec3):
shape = agxCollide.Box(agx.Vec3(0.5, 0.5, 0.5))
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
demoutils.create_visual(body)
return body
def create_cylinder(position: agx.Vec3):
shape = agxCollide.Cylinder(0.5, 1)
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
demoutils.create_visual(body)
body.setRotation(agx.EulerAngles(agx.PI_2, 0, 0))
return body
def add_second_joint_aft():
geometry = agxCollide.Geometry(second_joint_shape_aft.deepCopy(),
agx.AffineMatrix4x4.translate(0, 0, 100))
geometry.setLocalRotation(agx.EulerAngles(0, 0, math.pi / 2))
print(geometry.getRotation())
geometry.setEnableCollisions(True)
oneLegRobotApp.create_visual(geometry, agxRender.Color.Red())
oneLegRobotApp.add(geometry)
def create_cylinder(position: agx.Vec3, scale):
shape = agxCollide.Cylinder(0.5 * scale, 1 * scale)
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
oneLegRobotApp.create_visual(body)
body.setRotation(agx.EulerAngles(agx.PI_2, 0, 0))
return body
def __init__(self):
super().__init__()
upper_geometry = agxCollide.Geometry(upper_shape.deepCopy())
upper_geometry.setEnableCollisions(False)
snakeapp.create_visual(upper_geometry, agxRender.Color.Black())
self.add(upper_geometry)
def add_section_old(x, y, z, rotx, roty, rotz, path):
geometry = agxCollide.Geometry(path.deepCopy(),
agx.AffineMatrix4x4.translate(x, y, z))
geometry.setLocalRotation(agx.EulerAngles(rotx, roty, rotz))
#print(path + " were added with rotation " + geometry.getRotation())
geometry.setEnableCollisions(True)
oneLegRobotApp.create_visual(geometry, agxRender.Color.Red())
#oneLegRobotApp.add(geometry)
return geometry
def create_fenders(self, fender_material, r_fender, half_width, half_height, half_length):
fender_color = agxRender.Color.Black()
fender = agxCollide.Geometry(agxCollide.Capsule(0.2, half_width * 2))
fender.setPosition(half_length, 0, 0)
self.m_body.add(fender)
agxOSG.setDiffuseColor(agxOSG.createVisual(fender, demoutils.root()), fender_color)
fender.setMaterial(fender_material)
def create_section(self, x, y, z, rotx, roty, rotz, path):
geometry = agxCollide.Geometry(path.deepCopy(),
agx.AffineMatrix4x4.translate(x, y, z))
geometry.setLocalRotation(agx.EulerAngles(rotx, roty, rotz))
geometry.setEnableCollisions(True)
rigidBody = agx.RigidBody(geometry)
rigidBody.setMotionControl(agx.RigidBody.DYNAMICS)
oneLegRobotApp.create_visual(rigidBody, agxRender.Color.Red())
return rigidBody
def add_first_joint_aft():
geometry = agxCollide.Geometry(first_joint_shape_aft.deepCopy(),
agx.AffineMatrix4x4.translate(0, 0, 200))
geometry.setLocalRotation(agx.EulerAngles(0, 0, math.pi / 2))
print(geometry.getRotation())
geometry.setEnableCollisions(True)
rigidBody = agx.RigidBody(geometry)
oneLegRobotApp.create_visual(rigidBody, agxRender.Color.Red())
oneLegRobotApp.add(rigidBody)
rigidBody.setMotionControl(agx.RigidBody.STATIC)
def add_slope():
half_extents = agx.Vec3(length, 0.1, 0.005)
slope = agxCollide.Geometry(agxCollide.Box(half_extents),
agx.AffineMatrix4x4.translate(half_extents.x(), 0, -half_extents.z()))
slope.setRotation(agx.EulerAngles(0, -self._slope_angle, 0))
snakeapp.create_visual(slope, diffuse_color=agxRender.Color.Red())
slope.setMaterial(self.material)
self.add(slope)
return slope
def __init__(self):
super().__init__()
first_joint_geometry = agxCollide.Geometry(
first_joint_shape.deepCopy())
first_joint_geometry.setEnableCollisions(True)
oneLegRobotApp.create_visual(first_joint_geometry,
agxRender.Color.Black())
self.add(first_joint_geometry)
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 create_slope() -> agx.RigidBody:
slope_body = agx.RigidBody(agxCollide.Geometry(agxCollide.Box(1, 1, 0.05)))
snakeapp.create_visual(slope_body, diffuse_color=Color.Red())
# slope_body.setPosition(agx.Vec3(0, 0, 0))
slope_body.setRotation(agx.EulerAngles(0, -math.radians(slope_angle),
0)) # Rotate 30 deg around Y.
slope_body.setMotionControl(agx.RigidBody.STATIC)
material = agx.Material("slopeMaterial")
slope_body.getGeometries()[0].setMaterial(material)
return slope_body
