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)
예제 #2
0
    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)))
예제 #4
0
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)
예제 #5
0
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)
예제 #8
0
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
예제 #9
0
    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)
예제 #11
0
 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
예제 #12
0
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())
예제 #13
0
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
예제 #14
0
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)
예제 #16
0
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
예제 #17
0
    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
예제 #18
0
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
예제 #19
0
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
예제 #20
0
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)
예제 #21
0
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)
예제 #23
0
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
예제 #24
0
    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)
예제 #25
0
 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
예제 #26
0
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)
예제 #27
0
        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
예제 #28
0
    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)
예제 #29
0
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)
예제 #30
0
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