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: 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 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 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 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 __init__(self, props): self.brick_rb = agx.RigidBody() self.brick_rb.setName("duplobrick") self.height = props.height + props.stud_height #For broadcasting as sensor data self.props = props self.listeners = [] self.hole_sensors = []
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_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 build_rigid_body(geom: agxCollide.Geometry, name: str, pos, rot) -> agx.RigidBody: body = agx.RigidBody() body.add(geom) body.setName(name) body.setPosition(*pos) body.setRotation(agx.EulerAngles(*rot)) 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 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 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 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
def addball(sim, root, rad, pos, Fixed=True): if type(pos) == type([]): pos = agx.Vec3(pos[0], pos[1], pos[2]) ball = agx.RigidBody( agxCollide.Geometry( agxCollide.Sphere(rad))) ball.setPosition(pos) if(Fixed): ball.setMotionControl(1) sim.add(ball) agxOSG.setDiffuseColor(agxOSG.createVisual(ball, root), agxRender.Color.Red()) return ball
def addboxx(sim, root, dims, pos, Fixed=True, color = agxRender.Color.Red()): if type(pos) == type([]): pos = agx.Vec3(pos[0], pos[1], pos[2]) boxx = agx.RigidBody( agxCollide.Geometry( agxCollide.Box(dims[0]/2, dims[1]/2, dims[2]/2))) boxx.setPosition(pos) if(Fixed): boxx.setMotionControl(1) sim.add(boxx) agxOSG.setDiffuseColor(agxOSG.createVisual(boxx, root), color) return boxx
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_cylinderShape(MiroSystem, radius, height, density, pos, texture='test.jpg', scale=[1,1], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, color=[0.5, 0.5, 0.5]): '''system, size_x, size_y, size_z, pos, texture, scale = [5,5], hitbox = True/False''' # Convert position to chrono vector, supports using chvector as input as well agxSim = agxPython.getContext().environment.getSimulation() agxApp = agxPython.getContext().environment.getApplication() agxRoot = agxPython.getContext().environment.getSceneRoot() agxPos = agxVecify(pos) agxRotAxis = agxVecify(rotAxis) scale = scaleLimit(scale) # Create a cylinder body_geo = agxCollide.Geometry(agxCollide.Cylinder(radius, height)) body_geo.setName("body") body_geo.setEnableCollisions(Collide) body_cylinder = agx.RigidBody(body_geo) body_cylinder.getMassProperties().setMass(body_geo.calculateVolume()*density) if Fixed: body_cylinder.setMotionControl(1) body_cylinder.setPosition(agxPos) rotateBody(body_cylinder, rotX=-90) rotateBody(body_cylinder, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees) # Collision shape # if(Collide): # Visualization shape body_shape = agxOSG.createVisual(body_cylinder, agxRoot) # Body texture if texture: # Filter 'textures/' out of the texture name, it's added later if len(texture) > len('textures/'): if texture[0:len('textures/')] == 'textures/': texture = texture[len('textures/'):] if TEXTURES_ON: if texture not in LOADED_TEXTURES.keys(): agxTex = agxOSG.createTexture(TEXTURE_PATH+texture) LOADED_TEXTURES.update({texture: agxTex}) agxOSG.setTexture(body_shape, LOADED_TEXTURES[texture], True, agxOSG.DIFFUSE_TEXTURE, -scale[0], scale[1]) else: color = backupColor(texture, color) texture = False if not texture: agxColor = agxRender.Color(color[0], color[1], color[2]) agxOSG.setDiffuseColor(body_shape, agxColor) if len(color) > 3: agxOSG.setAlpha(body_shape, color[3]) agxSim.add(body_cylinder) return body_cylinder
def create_rigid_body_from_obj(name, scale, material): convexShapes = agxCollide.ConvexRefVector() convexBuilder = None agxUtil.createCdFromWavefrontOBJ(name, convexShapes, convexBuilder, scale) rb = agx.RigidBody() for c in convexShapes: assert (c) g = agxCollide.Geometry(c.asConvex()) if material: g.setMaterial(material) rb.add(g) return rb
def buildArena(sim, root): width = 14 arena_size = [width, width, 0.2] arena_pos = [0, 0, -1] h = 0.7 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()) sides = 8 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): 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()) obstacles(sim, root, arena_pos[2])
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 addcylinder(sim, root, dims, pos, Fixed=True, color = agxRender.Color.Red(), texture=False): if type(pos) == type([]): pos = agx.Vec3(pos[0], pos[1], pos[2]) cyl = agx.RigidBody( agxCollide.Geometry( agxCollide.Cylinder(dims[0], dims[1]))) cyl.setPosition(pos) if(Fixed): cyl.setMotionControl(1) sim.add(cyl) vis_body = agxOSG.createVisual(cyl, root) if texture: agxOSG.setTexture(vis_body, texture, True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0) else: agxOSG.setDiffuseColor(vis_body, color) return cyl
def create_floor(self): w = 200 b = 200 h = 10 # floor = agxCollide.Geometry(agxCollide.Box(2.5, 0.5, h), agx.AffineMatrix4x4.translate(0, 0, -h)) floor = agxCollide.Geometry(agxCollide.Box(w, b, h)) floor.setPosition(0, 0, 0) floor.setRotation(agx.EulerAngles(0, 0, 0)) floor.setEnableCollisions(True) rigidBody = agx.RigidBody(floor) rigidBody.setMotionControl(agx.RigidBody.STATIC) oneLegRobotApp.create_visual(rigidBody, diffuse_color=agxRender.Color.Green()) return floor
def add_table(sim, root, height, length, width, material, visual): """ Just a flat table """ table = agx.RigidBody() table.setMotionControl(agx.RigidBody.STATIC) table.setName("table") table_geom = agxCollide.Geometry( agxCollide.Box(length / 2, width / 2, height / 2)) table_geom.setName("table") table.add(table_geom) table_geom.setPosition(0, 0, -height / 2) table_geom.setMaterial(material) sim.add(table) if visual: m = agxOSG.createVisual(table, root) agxOSG.setDiffuseColor(m, agxRender.Color.SaddleBrown()) return table
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 addGround(MiroSystem, size_x, size_y, size_z, pos, heightmap, texture='test.jpg', scale=[4,3], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, mass=False, density=1000, dynamic=False, color=[0.5, 0.5, 0.5]): agxSim = agxPython.getContext().environment.getSimulation() agxApp = agxPython.getContext().environment.getApplication() agxRoot = agxPython.getContext().environment.getSceneRoot() # Create the ground ground_material = agx.Material("Ground") # Create the height field from a heightmap hf = agxCollide.HeightField.createFromFile("textures/"+heightmap, size_x, size_z, 0, size_y) ground_geometry = agxCollide.Geometry(hf) ground = agx.RigidBody(ground_geometry) ground.setPosition(agxVecify(pos)) ground.setMotionControl(agx.RigidBody.STATIC) node = agxOSG.createVisual( ground, agxRoot ) agxOSG.setShininess(node, 5) # Add a visual texture. agxOSG.setTexture(node, "textures/"+texture, True, agxOSG.DIFFUSE_TEXTURE, 100, 100) agxSim.add(ground)
def createLink(self, simulation, mesh, color): filename = "data/models/robots/Generic/" + mesh + ".obj" linkRb = agx.RigidBody(filename) mesh = agxUtil.createTrimeshFromWavefrontOBJ( filename, agxCollide.Trimesh.NO_WARNINGS, agx.Matrix3x3(), agx.Vec3()) if mesh is None: print("Unable to find file: " + filename) return None renderData = agxUtil.createRenderDataFromWavefrontOBJ( filename, agx.Matrix3x3(), agx.Vec3()) mesh.setRenderData(renderData) render_material = agxCollide.RenderMaterial() render_material.setDiffuseColor(color) renderData.setRenderMaterial(render_material) meshGeom = agxCollide.Geometry(mesh) linkRb.add(meshGeom) agxOSG.createVisual(meshGeom, agxPython.getContext().environment.getSceneRoot()) simulation.add(linkRb) agxUtil.addGroup(linkRb, self.robotGroupId) return linkRb
def createBeam(self, pos, length, rotY): agxSim = agxPython.getContext().environment.getSimulation() agxApp = agxPython.getContext().environment.getApplication() agxRoot = agxPython.getContext().environment.getSceneRoot() agxPos = agxVecify(pos) self.laser_geo = agxCollide.Geometry(agxCollide.Cylinder(0.004, length)) self.laser_geo.setName("body") # self.laser_geo.setEnableCollisions(False) self.laser_body = agx.RigidBody(self.laser_geo) self.laser_body.setMotionControl(1) self.laser_body.setPosition(agxPos) rotateBody(self.laser_body, rotY=rotY, rotDegrees=False) # Visualization shape self.laser_vis = agxOSG.createVisual(self.laser_body, agxRoot) agxOSG.setAlpha(self.laser_vis, 0.5) agxSim.add(self.laser_body) agxSim.add(self)