def add_color(geomerty): waterNode = agxOSG.createVisual(geomerty, demoutils.root()) color = agxRender.Color.DeepSkyBlue() alpha = 0.4 agxOSG.setDiffuseColor(waterNode, color) agxOSG.setAmbientColor(waterNode, agx.Vec4f(1)) agxOSG.setShininess(waterNode, 120) agxOSG.setAlpha(waterNode, alpha)
def setTriggered(self, trigger_status): self.triggered = trigger_status if self.triggered: agxColor = agxRender.Color(0, 1, 0) else: agxColor = agxRender.Color(1, 0, 0) agxOSG.setDiffuseColor(self.laser_vis, agxColor) agxOSG.setAlpha(self.laser_vis, 0.5)
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_visual(self, color: agxRender.Color = agxRender.Color.Gold()): """ Create visual given color. Arguments: color: agxRender.Color, agx.Vec4f - color of this tool """ from .environment import root node = agxOSG.createVisual(self, root()) agxOSG.setDiffuseColor(node, color) return node
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 create_water_visual(geo, root): node = agxOSG.createVisual(geo, root) diffuse_color = agxRender.Color(0.0, 0.75, 1.0, 1) ambient_color = agxRender.Color(1, 1, 1, 1) specular_color = agxRender.Color(1, 1, 1, 1) agxOSG.setDiffuseColor(node, diffuse_color) agxOSG.setAmbientColor(node, ambient_color) agxOSG.setSpecularColor(node, specular_color) agxOSG.setShininess(node, 120) agxOSG.setAlpha(node, 0.5) return node
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 _add_rendering(self, mode='osg'): # Set renderer self.app.setAutoStepping(True) self.app.setEnableDebugRenderer(False) self.app.setEnableOSGRenderer(True) file_directory = os.path.dirname(os.path.abspath(__file__)) package_directory = os.path.split(file_directory)[0] gripper_texture = os.path.join(package_directory, TEXTURE_GRIPPER_FILE) # Create scene graph for rendering root = self.app.getSceneRoot() rbs = self.sim.getRigidBodies() for rb in rbs: node = agxOSG.createVisual(rb, root) if rb.getName() == "hollow_cylinder": agxOSG.setDiffuseColor(node, agxRender.Color_SteelBlue()) agxOSG.setShininess(node, 15) elif rb.getName() == "gripper_body": agxOSG.setDiffuseColor(node, agxRender.Color(1.0, 1.0, 1.0, 1.0)) agxOSG.setTexture(node, gripper_texture, False, agxOSG.DIFFUSE_TEXTURE) agxOSG.setShininess(node, 2) elif "dlo" in rb.getName(): # Cable segments agxOSG.setDiffuseColor(node, agxRender.Color(0.0, 1.0, 0.0, 1.0)) else: agxOSG.setDiffuseColor(node, agxRender.Color.Beige()) agxOSG.setAlpha(node, 0.0) # Set rendering options scene_decorator = self.app.getSceneDecorator() scene_decorator.setEnableLogo(False) scene_decorator.setBackgroundColor(agxRender.Color(1.0, 1.0, 1.0, 1.0))
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 common_settings(self, sim, root, start_pos, visual, color): """ Common settings used for all bricks but that must be done after init """ self.brick_rb.getMassProperties().setMass(self.props.mass) self.brick_rb.setPosition(start_pos) self.set_amor_thresholds(1.e-3) sim.add(self.brick_rb) for listener in self.listeners: sim.add(listener) if visual: n = agxOSG.createVisual(self.brick_rb, root) agxOSG.setDiffuseColor(n, color)
def create_visual(obj, diffuse_color: Color = None, ambient_color: Color = None, shininess=None, alpha: float = None): node = agxOSG.createVisual(obj, root()) if diffuse_color is not None: agxOSG.setDiffuseColor(node, diffuse_color) if ambient_color is not None: agxOSG.setAmbientColor(node, ambient_color) if shininess is not None: agxOSG.setShininess(node, shininess) if alpha is not None: agxOSG.setAlpha(node, alpha) return node
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 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 add_rendering(sim): camera_distance = 0.5 light_pos = agx.Vec4(LENGTH / 2, -camera_distance, camera_distance, 1.) light_dir = agx.Vec3(0., 0., -1.) app = agxOSG.ExampleApplication(sim) app.setAutoStepping(False) app.setEnableDebugRenderer(False) app.setEnableOSGRenderer(True) root = app.getSceneRoot() rbs = sim.getRigidBodies() for rb in rbs: name = rb.getName() node = agxOSG.createVisual(rb, root) if name == "ground": agxOSG.setDiffuseColor(node, agxRender.Color.Gray()) elif "gripper_left" in name and "base" not in name: agxOSG.setDiffuseColor(node, agxRender.Color.Red()) elif "gripper_right" in name and "base" not in name: agxOSG.setDiffuseColor(node, agxRender.Color.Blue()) elif "dlo" in name: agxOSG.setDiffuseColor(node, agxRender.Color.Green()) else: agxOSG.setDiffuseColor(node, agxRender.Color.Beige()) agxOSG.setAlpha(node, 0.5) if "goal" in name: agxOSG.setAlpha(node, 0.2) scene_decorator = app.getSceneDecorator() light_source_0 = scene_decorator.getLightSource( agxOSG.SceneDecorator.LIGHT0) light_source_0.setPosition(light_pos) light_source_0.setDirection(light_dir) scene_decorator.setEnableLogo(False) return app
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 __init__(self, ground: agxCollide.Geometry, rov, depth): """ Args: ground: rov: depth: """ super().__init__() self.setMask(ContactEventListener.CONTACT) b = agxCollide.Box(.1, .1, depth) self.beam = Geometry(b) # print(self.beam.getShapes(),self.beam) self.beam.setPosition(0, 0, -depth) self.beam.setSensor(True) self.setFilter(GeometryFilter(self.beam, ground)) color = agxRender.Color.IndianRed() node = agxOSG.createVisual(self.beam, demoutils.root()) agxOSG.setDiffuseColor(node, color) agxOSG.setAmbientColor(node, agx.Vec4f(1)) agxOSG.setShininess(node, 120) agxOSG.setAlpha(node, 0.6) self.ground = ground.getShape().asHeightField()
def __init__(self, controller): super().__init__() self.controller = controller width = 2 * 1.5 right_fender = width * 0.25 length = 10 - right_fender * 0.4 ship_color = agxRender.Color.LightYellow() self.m_propulsion_force = 0 self.m_turn_fraction = 0.1 self.m_turn = 0 self.m_propulsion_step = 30 self.m_max_propulsion = 200000 self.m_min_propulsion = -500 self.init(width, length, right_fender) self.m_body.getMassProperties().setMass(1000 * 4) self.m_body.getCmFrame().setLocalTranslate(agx.Vec3(-0.2, 0, 0)) self.m_body.getGeometries()[0].setEnableCollisions(True) agxOSG.setDiffuseColor(agxOSG.createVisual(self.m_body, demoutils.root()), ship_color) """Create callbacks""" Sec.preCallback(lambda t: self.update_propulsion()) Sec.postCallback(lambda t: self.display_forces(t)) Sec.postCallback(lambda t: self.post(t))
def __init__(self): hf = agxCollide.HeightField.createFromFile("assets/textures/dirtRoad128.png", 6, 6, -0.5, 0.5) terrain = agxModel.Terrain(hf) snakeapp.add(terrain) self.material = agx.Material("GroundMaterial") terrain_data = terrain.getDataInterface() # type: agxModel.TerrainDataInterface terrain_data.setMaxCompressionMultiplier(32) particle_adhesion = 0 deformity = 100 angle_of_repose = math.pi * 0.2 material_index = 0 terrain_data.add(self.material, material_index, particle_adhesion, deformity, angle_of_repose) range_for_youngs_modulus = agx.RangeReal(10000, 20000) terrain_renderer = agxOSG.TerrainRenderer(terrain, range_for_youngs_modulus, snakeapp.root()) snakeapp.add(terrain_renderer) terrain_visual = terrain_renderer.getTerrainNode() agxOSG.setDiffuseColor(terrain_visual, Color(0.7, 0.7, 0.8, 1)) agxOSG.setSpecularColor(terrain_visual, Color(0.4, 0.4, 0.4, 1)) agxOSG.setShininess(terrain_visual, 128)
def buildScene1(self, app, sim, root): # Create the Terrain num_cells_x = 80 num_cells_y = 80 cell_size = 0.15 max_depth = 1.0 agx_heightField = agxCollide.HeightField(num_cells_x, num_cells_y, (num_cells_x - 1) * cell_size, (num_cells_y - 1) * cell_size) # Define the initial height field (random or fixed) depending on if if data collection or test if self.control_mode == "data_collection": np_heightField = self.createRandomHeightfield( num_cells_x, num_cells_y, cell_size) elif self.control_mode == "mpcc" or self.control_mode == "trajectory_control": np_heightField = self.createRandomHeightfield( num_cells_x, num_cells_y, cell_size) if self.set_height_from_previous: agx_heightField = self.agx_heightField_previous else: agx_heightField = self.setHeightField(agx_heightField, np_heightField) terrain = agxTerrain.Terrain.createFromHeightField( agx_heightField, 5.0) sim.add(terrain) # Define Gravity G = agx.Vec3(0, 0, -10.0) sim.setUniformGravity(G) # define the material terrain.loadLibraryMaterial("sand_1") terrainMaterial = terrain.getTerrainMaterial() terrainMaterial.getBulkProperties().setSwellFactor(1.00) compactionProperties = terrainMaterial.getCompactionProperties() compactionProperties.setAngleOfReposeCompactionRate(500.0) terrain.setCompaction(1.05) # The trenching will reach the bounds of the terrain so we simply remove particles out of bounds # to get rid of the mateiral in a practical way. terrain.getProperties().setDeleteSoilParticlesOutsideBounds(True) if app: # Setup a renderer for the terrain renderer = agxOSG.TerrainVoxelRenderer(terrain, root) renderer.setRenderHeightField(True) # We choose to render the compaction of the soil to visually denote excavated # soil from compacted ground # renderer.setRenderCompaction( True, agx.RangeReal( 1.0, 1.05 ) ) renderer.setRenderHeights(True, agx.RangeReal(-0.4, 0.1)) # renderer.setRenderHeights(True, agx.RangeReal(-0.5,0.5)) renderer.setRenderVoxelSolidMass(False) renderer.setRenderVoxelFluidMass(False) renderer.setRenderNodes(False) renderer.setRenderVoxelBoundingBox(False) renderer.setRenderSoilParticlesMesh(True) sim.add(renderer) # Set contact materials of the terrain and shovel # This contact material governs the resistance that the shovel will feel when digging into the terrain # [ Shovel - Terrain ] contact material shovelMaterial = agx.Material("shovel_material") terrainMaterial = terrain.getMaterial( agxTerrain.Terrain.MaterialType_TERRAIN) shovelTerrainContactMaterial = agx.ContactMaterial( shovelMaterial, terrainMaterial) shovelTerrainContactMaterial.setYoungsModulus(1e8) shovelTerrainContactMaterial.setRestitution(0.0) shovelTerrainContactMaterial.setFrictionCoefficient(0.4) sim.add(shovelTerrainContactMaterial) # Create the trenching shovel body creation, do setup in the Terrain object and # constrain it to a kinematic that will drive the motion # Create the bucket rigid body cuttingEdge, topEdge, forwardVector, bucket = self.createBucket( default) sim.add(bucket) # Create the Shovel object using the previously defined cutting and top edge shovel = agxTerrain.Shovel(bucket, topEdge, cuttingEdge, forwardVector) agxUtil.setBodyMaterial(bucket, shovelMaterial) # Set a margin around the bounding box of the shovel where particles are not to be merged shovel.setNoMergeExtensionDistance(0.1) # Add the shovel to the terrain terrain.add(shovel) if app: # and self.consecutive_scoop_i < 4: # Create visual representation of the shovel node = agxOSG.createVisual(bucket, root) agxOSG.setDiffuseColor(node, agxRender.Color.Gold()) agxOSG.setAlpha(node, 1.0) # Set initial bucket rotation if self.control_mode == "mpcc": angle_bucket_initial = -0.3 * np.pi else: angle_bucket_initial = np.random.uniform(low=-0.25 * np.pi, high=-0.35 * np.pi) bucket.setRotation(agx.EulerAngles(0.0, angle_bucket_initial, agx.PI)) # Get the offset of the bucket tip from the COM tip_offset = shovel.getCuttingEdgeWorld().p2 # inertia_tensor = bucket.getMassProperties().getInertiaTensor() mass = bucket.getMassProperties().getMass() h_offset_sqrd = tip_offset[0]**2 + tip_offset[2]**2 inertia_bucket = inertia_tensor.at(1, 1) + mass * h_offset_sqrd # Set initial bucket position (for consecutive scoops) if self.control_mode == "mpcc" or self.control_mode == "trajectory_control": if self.consecutive_scoop_i == 0: x_initial_tip = -4.0 elif self.consecutive_scoop_i == 1: x_initial_tip = -3.6 elif self.consecutive_scoop_i == 2: x_initial_tip = -3.3 else: x_initial_tip = -2.6 else: x_initial_tip = np.random.uniform(low=-4.5, high=-3.0) # find the soil height at the initial penetration location hf_grid_initial = terrain.getClosestGridPoint( agx.Vec3(x_initial_tip, 0.0, 0.0)) height_initial = terrain.getHeight(hf_grid_initial) - 0.05 # Set the initial bucket location such that it is just contacting the soil position = agx.Vec3(x_initial_tip - tip_offset[0], 0, height_initial - tip_offset[2]) bucket.setPosition(terrain.getTransform().transformPoint(position)) bucket.setVelocity(0.0, 0.0, 0.0) # bucket.setAngularVelocity(0.0, 0.05, 0.0) # Add a lockjoint between a kinematic sphere and the shovel # in order to have some compliance when moving the shovel # through the terrain offset = agx.Vec3(0.0, 0.0, 0.0) ## ADD ALL THE JOINTS TO CONTROL THE BUCKET (x,z,theta) sphere1 = agx.RigidBody(agxCollide.Geometry(agxCollide.Sphere(.1))) sphere2 = agx.RigidBody(agxCollide.Geometry(agxCollide.Sphere(.1))) sphere3 = agx.RigidBody(agxCollide.Geometry(agxCollide.Sphere(.1))) sphere1.setMotionControl(agx.RigidBody.DYNAMICS) sphere2.setMotionControl(agx.RigidBody.DYNAMICS) sphere3.setMotionControl(agx.RigidBody.DYNAMICS) sphere1.getGeometries()[0].setEnableCollisions(False) sphere2.getGeometries()[0].setEnableCollisions(False) sphere3.getGeometries()[0].setEnableCollisions(False) tip_position = shovel.getCuttingEdgeWorld().p2 tip_position[1] = bucket.getCmPosition()[1] sphere1.setPosition(tip_position) sphere2.setPosition(tip_position) sphere3.setPosition(tip_position) sphere1.getMassProperties().setMass(0.000001) sphere2.getMassProperties().setMass(0.000001) sphere3.getMassProperties().setMass(0.000001) # print('sphere mass: ', sphere1.getMassProperties().getMass()) sim.add(sphere1) sim.add(sphere2) sim.add(sphere3) # Set prismatic joint for x transalation world - sphere 1 f1 = agx.Frame() f1.setLocalRotate(agx.EulerAngles(0, math.radians(90), 0)) prismatic1 = agx.Prismatic(sphere1, f1) # Set prismatic joint for z transalation world - sphere 2 f1 = agx.Frame() f2 = agx.Frame() f1.setLocalRotate(agx.EulerAngles(0, math.radians(180), 0)) f2.setLocalRotate(agx.EulerAngles(0, math.radians(180), 0)) prismatic2 = agx.Prismatic(sphere1, f1, sphere2, f2) # # Set hinge joint for rotation of the bucket f1 = agx.Frame() f1.setLocalRotate(agx.EulerAngles(-math.radians(90), 0, 0)) f2 = agx.Frame() f2.setLocalRotate(agx.EulerAngles(-math.radians(90), 0, 0)) hinge2 = agx.Hinge(sphere2, f1, sphere3, f2) sim.add(prismatic1) sim.add(prismatic2) sim.add(hinge2) lock = agx.LockJoint(sphere3, bucket) sim.add(lock) # Uncomment to lock rotations # sim.add(agx.LockJoint(sphere2,bucket)) # constant force and torque operations = [agx.Vec3(0.0, 0.0, 0.0), agx.Vec3(0.0, 0.0, 0.0)] # Extract soil shape along the bucket excavation direction x_hf, z_hf = self.extractSoilSurface(terrain, sim) self.soilShapeEvaluator = SoilSurfaceEvaluator(x_hf, z_hf) setattr(self.dfl, "soilShapeEvaluator", self.soilShapeEvaluator) if self.control_mode == "data_collection": # create driver and add it to the simulation driver = ForceDriverPID(app, sphere3, lock, hinge2, prismatic1, prismatic2, terrain, shovel, operations, self.dt_control) # Add the current surface evaluator to the controller setattr(driver, "soilShapeEvaluator", self.soilShapeEvaluator) # Add the controller to the simulation sim.add(driver) elif self.control_mode == "trajectory_control": # create driver and add it to the simulation # create driver and add it to the simulation driver = ForceDriverTrajectory(app, sphere3, lock, hinge2, prismatic1, prismatic2, terrain, shovel, operations, self.dt_control) # Add the current surface evaluator to the controller setattr(driver, "soilShapeEvaluator", self.soilShapeEvaluator) setattr(driver, "dfl", self.dfl) x_path = x_initial_tip + np.array( [0., 0.5, 1.5, 2.0, 2.5, 3.0, 3.5]) y_soil, _, _, _ = self.soilShapeEvaluator.soil_surf_eval(x_path) y_path = y_soil + np.array( [-0.07, -0.25, -0.25, -0.25, -0.25, -0.25, -0.02]) spl_path = spline_path(x_path, y_path) setattr(driver, "path_eval", spl_path.path_eval) # Add the controller to the simulation sim.add(driver) elif self.control_mode == "mpcc": # create driver and add it to the simulation driver = ForceDriverDFL(app, sphere3, lock, hinge2, terrain, shovel, self.dt_control) ################ MPCC CONTROLLER ############################ # Add the current surface evaluator to the controller setattr(driver, "soilShapeEvaluator", self.soilShapeEvaluator) setattr(driver, "dfl", self.dfl) setattr(driver, "scaling", self.scaling) x_path = x_initial_tip + np.array([ 0., 0.5, 1.5, 2.0, 2.5, 3.0, ]) y_soil, _, _, _ = self.soilShapeEvaluator.soil_surf_eval(x_path) y_path = y_soil + np.array( [-0.07, -0.25, -0.25, -0.25, -0.25, -0.02]) # Set the state constraints if self.observable_type == "dfl": x_min = np.array([ x_initial_tip - 0.1, -3., 0.5, -0.5, -2.5, -2.5, -80000 * self.scaling, -80000 * self.scaling, 0.0, -70000 * self.scaling, -70000 * self.scaling, -70000 * self.scaling, -70000 * self.scaling ]) x_max = np.array([ 2., 5.0, 2.5, 2.5, 2.5, 2.5, 80000 * self.scaling, 80000 * self.scaling, 3000. * self.scaling, 70000 * self.scaling, 70000 * self.scaling, 70000 * self.scaling, 70000 * self.scaling ]) n_dyn = self.dfl.plant.n elif self.observable_type == "x": x_min = np.array( [x_initial_tip - 0.1, -3., 0.5, -0.5, -2.5, -2.5]) x_max = np.array([2., 5.0, 2.5, 2.5, 2.5, 2.5]) n_dyn = self.dfl.plant.n_x # Set the input constraints u_min = np.array([ -100. * self.scaling, -70000 * self.scaling, -70000 * self.scaling ]) u_max = np.array([ 75000. * self.scaling, 70000 * self.scaling, 70000 * self.scaling ]) if self.set_height_from_previous: pass else: self.spl_path = spline_path(x_path, y_path) # instantiate the MPCC object mpcc = MPCC(np.zeros((n_dyn, n_dyn)), np.zeros((n_dyn, self.dfl.plant.n_u)), x_min, x_max, u_min, u_max, dt=self.dt_data, N=50) # set the observation function, path object and linearization function setattr(mpcc, "path_eval", self.spl_path.path_eval) setattr(mpcc, "get_soil_surface", self.soilShapeEvaluator.soil_surf_eval) if self.model_has_surface_shape: print('Linearizing with soil shape') setattr(mpcc, "get_linearized_model", self.dfl.linearize_soil_dynamics_koop) else: setattr(mpcc, "get_linearized_model", self.dfl.linearize_soil_dynamics_no_surface) self.mpcc = copy.copy(mpcc) pos_tip, vel_tip, acl_tip, ang_tip, omega, alpha = measureBucketState( sphere3, shovel) x_i = np.array([ pos_tip[0], pos_tip[2], ang_tip, vel_tip[0], vel_tip[2], omega[1] ]) eta_i = np.array( [acl_tip[0], acl_tip[2], alpha[1], 0., 0., 0., 0.]) # Choose the initial path arcposition based on a close initial x tip position theta_array = np.linspace(-10, 0, num=1000) for i in range(len(theta_array)): x_path, y_path = self.spl_path.path_eval(theta_array[i], d=0) if x_path > pos_tip[0]: path_initial = theta_array[i] break # set initial input (since input cost is differential) x_0_mpcc = np.concatenate( (self.dfl.g_Koop(x_i, eta_i, _), np.array([path_initial]))) u_minus_mpcc = np.array([0.0, 0.0, 0.0, 0.0]) driver.last_x_opt = x_0_mpcc # sets up the new mpcc problem _mpcc mpcc.setup_new_problem(self.Q_mpcc, self.R_mpcc, self.q_theta_mpcc, x_0_mpcc, u_minus_mpcc) setattr(driver, "mpcc", mpcc) ##################################################################################### # Add the controller to the simulation sim.add(driver) # Limit core usage to number of physical cores. Assume that HT/SMT is active # and divide max threads with 2. agx.setNumThreads(0) n = int(agx.getNumThreads() / 2 - 1) agx.setNumThreads(n) # Setup initial camera view if app: createHelpText(sim, app) return terrain, shovel, driver, sphere3
def add_rendering(sim): camera_distance = 0.5 light_pos = agx.Vec4(LENGTH / 2, - camera_distance, camera_distance, 1.) light_dir = agx.Vec3(0., 0., -1.) app = agxOSG.ExampleApplication(sim) app.setAutoStepping(True) app.setEnableDebugRenderer(False) app.setEnableOSGRenderer(True) scene_decorator = app.getSceneDecorator() light_source_0 = scene_decorator.getLightSource(agxOSG.SceneDecorator.LIGHT0) light_source_0.setPosition(light_pos) light_source_0.setDirection(light_dir) root = app.getRoot() rbs = sim.getRigidBodies() for rb in rbs: name = rb.getName() node = agxOSG.createVisual(rb, root) if name == "ground": agxOSG.setDiffuseColor(node, agxRender.Color.Gray()) elif name == "pusher": agxOSG.setDiffuseColor(node, agxRender.Color(0.0, 0.0, 1.0, 1.0)) elif "obstacle" in name: agxOSG.setDiffuseColor(node, agxRender.Color(1.0, 0.0, 0.0, 1.0)) elif "dlo" in name: agxOSG.setDiffuseColor(node, agxRender.Color(0.0, 1.0, 0.0, 1.0)) elif "bounding_box" in name: agxOSG.setDiffuseColor(node, agxRender.Color.Burlywood()) else: # Base segments agxOSG.setDiffuseColor(node, agxRender.Color.Beige()) agxOSG.setAlpha(node, 0.2) if "goal" in name: agxOSG.setAlpha(node, 0.2) scene_decorator = app.getSceneDecorator() light_source_0 = scene_decorator.getLightSource(agxOSG.SceneDecorator.LIGHT0) light_source_0.setPosition(light_pos) light_source_0.setDirection(light_dir) scene_decorator.setEnableLogo(False) return app
def add_rendering(sim): app = agxOSG.ExampleApplication(sim) # Set renderer app.setAutoStepping(True) app.setEnableDebugRenderer(False) app.setEnableOSGRenderer(True) # Create scene graph for rendering root = app.getSceneRoot() rbs = sim.getRigidBodies() for rb in rbs: node = agxOSG.createVisual(rb, root) if rb.getName() == "ground": agxOSG.setDiffuseColor(node, agxRender.Color(0.8, 0.8, 0.8, 1.0)) elif rb.getName() == "walls": agxOSG.setDiffuseColor(node, agxRender.Color.Burlywood()) elif rb.getName() == "cylinder": agxOSG.setDiffuseColor(node, agxRender.Color.DarkGray()) elif rb.getName() == "cylinder_inner": agxOSG.setDiffuseColor(node, agxRender.Color.LightSteelBlue()) elif rb.getName() == "gripper_0" or rb.getName() == "gripper_1": agxOSG.setDiffuseColor(node, agxRender.Color(0.1, 0.1, 0.1, 1.0)) elif "dlo" in rb.getName(): # Cable segments agxOSG.setDiffuseColor(node, agxRender.Color(0.1, 0.5, 0.0, 1.0)) agxOSG.setAmbientColor(node, agxRender.Color(0.2, 0.5, 0.0, 1.0)) elif rb.getName() == "obstacle": agxOSG.setDiffuseColor(node, agxRender.Color(0.5, 0.5, 0.5, 1.0)) elif rb.getName() == "obstacle_goal": agxOSG.setDiffuseColor(node, agxRender.Color(0.0, 0.0, 1.0, 1.0)) else: agxOSG.setDiffuseColor(node, agxRender.Color.Beige()) agxOSG.setAlpha(node, 0.0) # Set rendering options scene_decorator = app.getSceneDecorator() scene_decorator.setEnableLogo(False) scene_decorator.setBackgroundColor(agxRender.Color(1.0, 1.0,1.0, 1.0)) return app
def create_visual(obj, **kwargs): """ Create visual given object. World for any agxOSG.createVisual supported object but handles agxTerrain.Terrain instances differently by creating a TerrainVoxelRenderer instance and adding it to the simulation. Terrain instance: Arguments: obj: agxTerrain.Terrain instance render_particles: bool - True to render particles, False to not render particles (default: True). render_particles_mesh: bool - True to render particles as triangle meshes (default: False). render_heights: bool - True to render heights color range (default: False). render_compaction: bool - True to render compaction color range (default: False). render_voxel_compaction: bool - True to render voxel compaction from grid (default: False). render_height_field: bool - True to render height field, False to not render height field (default: True). render_solid_mass: bool - True to render solid mass, False to not render solid mass (default: False). render_fluid_mass: bool - True to render fluid mass, False to not render fluid mass (default: False). render_nodes: bool - Unknown (default: False). render_bounding_box - True to render voxel bounding boxes, False to not (default: False). heights_range: agx.RangeReal - Heights range when render_heights = True (default: (-inf, inf)). compaction_range: agx.RangeReal - Compaction range when render_compaction = True (default: [0.75, 1.5]). Other instances: Arguments: obj: agxCollide.Geometry, agx.RigidBody, agx.ParticleSystem, agx.GranularBodySystem - object instance color: agxRender.Color, agx.Vec4f - color (default: agxRender.Color.Gold) """ from .environment import root if isinstance(obj, (TerrainTool, TerrainToolBody)): return obj.create_visual( color=kwargs.get('color', agxRender.Color.Gold())) elif isinstance(obj, agxTerrain.Terrain): from .environment import simulation render_particles = kwargs.get('render_particles', True) render_particles_mesh = kwargs.get('render_particles_mesh', False) render_heights = kwargs.get('render_heights', False) render_compaction = kwargs.get('render_compaction', False) render_voxel_compaction = kwargs.get('render_voxel_compaction', False) render_height_field = kwargs.get('render_height_field', True) render_solid_mass = kwargs.get('render_solid_mass', False) render_fluid_mass = kwargs.get('render_fluid_mass', False) render_nodes = kwargs.get('render_nodes', False) render_bounding_box = kwargs.get('render_bounding_box', False) heights_range = kwargs.get('heights_range', agx.RangeReal()) compaction_range = kwargs.get('compaction_range', agx.RangeReal(0.75, 1.5)) renderer = agxOSG.TerrainVoxelRenderer(obj, root()) renderer.setName('terrain_renderer') renderer.setRenderSoilParticles(render_particles) renderer.setRenderSoilParticlesMesh(render_particles_mesh) renderer.setRenderHeights(render_heights, heights_range) renderer.setRenderCompaction(render_compaction, compaction_range) renderer.setRenderVoxelCompaction(render_voxel_compaction) renderer.setRenderHeightField(render_height_field) renderer.setRenderVoxelSolidMass(render_solid_mass) renderer.setRenderVoxelFluidMass(render_fluid_mass) renderer.setRenderNodes(render_nodes) renderer.setRenderVoxelBoundingBox(render_bounding_box) simulation().add(renderer) return renderer else: node = agxOSG.createVisual(obj, root()) if 'color' in kwargs: agxOSG.setDiffuseColor(node, kwargs['color']) return node
def _add_rendering(self, mode='osg'): # Set renderer self.app.setAutoStepping(True) self.app.setEnableDebugRenderer(False) self.app.setEnableOSGRenderer(True) # Create scene graph for rendering root = self.app.getSceneRoot() rbs = self.sim.getRigidBodies() for rb in rbs: node = agxOSG.createVisual(rb, root) if rb.getName() == "ground": agxOSG.setDiffuseColor(node, agxRender.Color.SlateGray()) elif rb.getName() == "cylinder_top_0" or rb.getName( ) == "cylinder_top_1" or rb.getName() == "cylinder_top_2": agxOSG.setDiffuseColor(node, agxRender.Color.DarkGray()) elif rb.getName() == "cylinder_inner_0" or rb.getName( ) == "cylinder_inner_1" or rb.getName() == "cylinder_inner_2": agxOSG.setDiffuseColor(node, agxRender.Color.LightSteelBlue()) elif rb.getName() == "cylinder_low_0" or rb.getName( ) == "cylinder_low_1" or rb.getName() == "cylinder_low_2": agxOSG.setDiffuseColor(node, agxRender.Color.DarkGray()) elif rb.getName() == "gripper": agxOSG.setDiffuseColor(node, agxRender.Color.DarkBlue()) elif "dlo" in rb.getName(): # Cable segments agxOSG.setDiffuseColor(node, agxRender.Color(0.8, 0.2, 0.2, 1.0)) else: agxOSG.setDiffuseColor(node, agxRender.Color.Beige()) agxOSG.setAlpha(node, 0.0) # Set rendering options scene_decorator = self.app.getSceneDecorator() scene_decorator.setEnableLogo(False) scene_decorator.setBackgroundColor(agxRender.Color(1.0, 1.0, 1.0, 1.0))
def buildBot(sim, root, bot_pos, controller='Arrows', drivetrain = 'FWD', color=False): body_wid = 0.32 body_len = 0.6 body_hei = 0.16 wheel_rad = 0.07 wheel_wid = 0.02 wheel_dmp = -0.02 body = agx.RigidBody( agxCollide.Geometry( agxCollide.Box(body_wid/2, body_len/2, body_hei/2))) body.setPosition(bot_pos[0], bot_pos[1], bot_pos[2] + body_hei/2 + wheel_rad + wheel_dmp ) # body.setMotionControl(1) body.setRotation(agx.Quat(np.pi, agx.Vec3(0,0,1))) sim.add(body) vis_body = agxOSG.createVisual(body, root) if color: agxOSG.setDiffuseColor(vis_body, color) else: agxOSG.setTexture(vis_body, 'flames.png', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0) wheelLF = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(wheel_rad, wheel_wid))) wheelLF.setPosition(bot_pos[0]-(body_wid/2+wheel_wid/2), bot_pos[1]+(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad) # wheelLF.setMotionControl(1) wheelLF.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1))) sim.add(wheelLF) # agxOSG.setDiffuseColor(agxOSG.createVisual(wheelLF, root), agxRender.Color.Red()) wheelRF = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(wheel_rad, wheel_wid))) wheelRF.setPosition(bot_pos[0]+(body_wid/2+wheel_wid/2), bot_pos[1]+(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad) # wheelRF.setMotionControl(1) wheelRF.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1))) sim.add(wheelRF) # agxOSG.setDiffuseColor(agxOSG.createVisual(wheelRF, root), agxRender.Color.Red()) wheelLB = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(wheel_rad, wheel_wid))) wheelLB.setPosition(bot_pos[0]-(body_wid/2+wheel_wid/2), bot_pos[1]-(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad) # wheelLB.setMotionControl(1) wheelLB.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1))) sim.add(wheelLB) # agxOSG.setDiffuseColor(agxOSG.createVisual(wheelLB, root), agxRender.Color.Red()) wheelRB = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(wheel_rad, wheel_wid))) wheelRB.setPosition(bot_pos[0]+(body_wid/2+wheel_wid/2), bot_pos[1]-(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad) # wheelRB.setMotionControl(1) wheelRB.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1))) sim.add(wheelRB) # agxOSG.setDiffuseColor(agxOSG.createVisual(wheelRB, root), agxRender.Color.Red()) for wheel in [wheelLB, wheelLF, wheelRB, wheelRF]: vis_body = agxOSG.createVisual(wheel, root) agxOSG.setTexture(vis_body, 'tire.png', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0) light_rad = 0.02 light_dep = 0.01 headlightL = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(light_rad, light_dep))) headlightL.setPosition( bot_pos[0] + 0.79*body_wid/2, bot_pos[1] + body_len/2+light_dep/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp) # headlightL.setMotionControl(1) # headlightL.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1))) sim.add(headlightL) agxOSG.setTexture(agxOSG.createVisual(headlightL, root), 'light.png', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0) hf = agx.HingeFrame() hf.setAxis(agx.Vec3(0,1,0)) hf.setCenter(agx.Vec3( bot_pos[0] + 0.79*body_wid/2, bot_pos[1] + body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp )) HLL = agx.Hinge(hf, body, headlightL) sim.add(HLL) headlightR = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(light_rad, light_dep))) headlightR.setPosition(bot_pos[0] -0.79*body_wid/2, bot_pos[1] + body_len/2+light_dep/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp ) # headlightR.setMotionControl(1) # headlightL.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1))) sim.add(headlightR) agxOSG.setTexture(agxOSG.createVisual(headlightR, root), 'light.png', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0) hf = agx.HingeFrame() hf.setAxis(agx.Vec3(0,1,0)) hf.setCenter(agx.Vec3(bot_pos[0] -0.79*body_wid/2, bot_pos[1] + body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp )) HLR = agx.Hinge(hf, body, headlightR) sim.add(HLR) light_wid = 0.012 light_hei = 0.02 light_dep = 0.003 taillightL = agx.RigidBody(agxCollide.Geometry( agxCollide.Box(light_wid, light_dep, light_hei))) taillightL.setPosition(bot_pos[0] -0.79*body_wid/2,bot_pos[1] -(body_len/2+light_dep/2), bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp) # taillightL.setMotionControl(1) # headlightL.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1))) sim.add(taillightL) agxOSG.setDiffuseColor(agxOSG.createVisual(taillightL, root), agxRender.Color.Red()) hf = agx.HingeFrame() hf.setAxis(agx.Vec3(0,1,0)) hf.setCenter(agx.Vec3(bot_pos[0] -0.79*body_wid/2, bot_pos[1] -body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp + light_hei/3)) TLL_hi = agx.Hinge(hf, body, taillightL) sim.add(TLL_hi) hf = agx.HingeFrame() hf.setAxis(agx.Vec3(0,1,0)) hf.setCenter(agx.Vec3(bot_pos[0] -0.79*body_wid/2, bot_pos[1] -body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp - light_hei/3)) TLL_low = agx.Hinge(hf, body, taillightL) sim.add(TLL_low) taillightR = agx.RigidBody(agxCollide.Geometry( agxCollide.Box(light_wid, light_dep, light_hei))) taillightR.setPosition( bot_pos[0] + 0.79*body_wid/2,bot_pos[1] -(body_len/2+light_dep/2), bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp) # taillightR.setMotionControl(1) # headlightL.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1))) sim.add(taillightR) agxOSG.setDiffuseColor(agxOSG.createVisual(taillightR, root), agxRender.Color.Red()) hf = agx.HingeFrame() hf.setAxis(agx.Vec3(0,1,0)) hf.setCenter(agx.Vec3( bot_pos[0] + 0.79*body_wid/2,bot_pos[1]-body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp + light_hei/3)) TLR = agx.Hinge(hf, body, taillightR) sim.add(TLR) hf = agx.HingeFrame() hf.setAxis(agx.Vec3(0,1,0)) hf.setCenter(agx.Vec3( bot_pos[0] + 0.79*body_wid/2,bot_pos[1]-body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp - light_hei/3)) TLR = agx.Hinge(hf, body, taillightR) sim.add(TLR) windangle = np.pi/4 windshield = agx.RigidBody( agxCollide.Geometry( agxCollide.Box(0.9*body_wid/2, 0.005, body_hei/3))) windshield.setPosition(bot_pos[0], bot_pos[1]+body_len/5, bot_pos[2] + body_hei + wheel_rad + wheel_dmp + np.cos(windangle)*body_hei/3) # windshield.setTorque(0,0,100) # windshield.setMotionControl(2) windshield.setRotation(agx.Quat(windangle, agx.Vec3(1,0,0))) sim.add(windshield) agxOSG.setTexture(agxOSG.createVisual(windshield, root), 'windshield.jpg', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0) hf = agx.HingeFrame() hf.setAxis(agx.Vec3(0,0,1)) hf.setCenter(agx.Vec3(-body_wid/3, body_len/5, bot_pos[2] + body_hei + wheel_rad + wheel_dmp)) windh1 = agx.Hinge(hf, body, windshield) sim.add(windh1) hf = agx.HingeFrame() hf.setAxis(agx.Vec3(0,0,1)) hf.setCenter(agx.Vec3(body_wid/3, body_len/5, bot_pos[2] + body_hei + wheel_rad + wheel_dmp)) windh2 = agx.Hinge(hf, body, windshield) sim.add(windh2) x_ax = agx.Vec3(1,0,0) y_ax = agx.Vec3(0,1,0) z_ax = agx.Vec3(0,0,1) hf = agx.HingeFrame() hf.setAxis(agx.Vec3(-1,0,0)) hf.setCenter(agx.Vec3(bot_pos[0]-body_wid/2, bot_pos[1]+(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad)) axleLF = agx.Hinge(hf, body, wheelLF) sim.add(axleLF) hf = agx.HingeFrame() hf.setAxis(agx.Vec3(-1,0,0)) hf.setCenter(agx.Vec3(bot_pos[0]-body_wid/2, bot_pos[1]-(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad)) axleLB = agx.Hinge(hf, body, wheelLB) sim.add(axleLB) hf = agx.HingeFrame() hf.setAxis(agx.Vec3( 1,0,0)) hf.setCenter(agx.Vec3(bot_pos[0]+body_wid/2, bot_pos[1]-(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad)) axleRB = agx.Hinge(hf, body, wheelRB) sim.add(axleRB) hf = agx.HingeFrame() hf.setAxis(agx.Vec3( 1,0,0)) hf.setCenter(agx.Vec3(bot_pos[0]+body_wid/2, bot_pos[1]+(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad)) axleRF = agx.Hinge(hf, body, wheelRF) sim.add(axleRF) wheels = [wheelLF, wheelRF] # default FWD if drivetrain == 'FWD': wheels = [wheelLF, wheelRF] elif drivetrain == 'RWD': wheels = [wheelLB, wheelRB] elif drivetrain == 'AWD': wheels = [wheelLF, wheelRF, wheelLB, wheelRB] if controller == 'Numpad': sim.add(WheelControllerNumpad(wheels)) sim.add(Fjoink(body).setHopper(agxSDK.GuiEventListener.KEY_KP_Subtract)) else: sim.add(WheelControllerArrows(wheels)) sim.add(Fjoink(body)) # return a pointer to the body return body
def add_boxShape(MiroSystem, size_x, size_y, size_z, pos, texture=False, 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], friction=False): '''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) [size_x, size_y, size_z] = xyzTransform([size_x, size_y, size_z], True) scale = [scale[0]/4, scale[1]/3] # Create a box body_geo = agxCollide.Geometry( agxCollide.Box(size_x/2, size_y/2, size_z/2)) body_geo.setName("body") if friction: high_friction_tires = agx.Material('Tires', 0.05, friction) body_geo.setMaterial(high_friction_tires) body_geo.setEnableCollisions(Collide) body_box = agx.RigidBody(body_geo) if mass: body_box.getMassProperties().setMass(mass) else: body_box.getMassProperties().setMass(body_geo.calculateVolume()*density) if Fixed: body_box.setMotionControl(1) body_box.setPosition(agxPos) rotateBody(body_box, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees) # Collision shape # if(Collide): # Visualization shape body_shape = agxOSG.createVisual(body_box, 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 or texture in important_textures: if texture not in LOADED_TEXTURES.keys(): agxTex = agxOSG.createTexture(TEXTURE_PATH+texture) LOADED_TEXTURES.update({texture: agxTex}) if TEXTURE_PATH == 'textures_lowres/' and texture=='yellow_brick.jpg': scale[0] = 11*scale[0] scale[1] = 8*scale[1] agxOSG.setTexture(body_shape, TEXTURE_PATH+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_box) return body_box
def _add_rendering(self): self.app.setAutoStepping(False) if not self.root: self.root = self.app.getRoot() rbs = self.sim.getRigidBodies() for rb in rbs: name = rb.getName() node = agxOSG.createVisual(rb, self.root) if name == "ground": agxOSG.setDiffuseColor(node, agxRender.Color.SlateGray()) elif "gripper_left" in name and "base" not in name: agxOSG.setDiffuseColor(node, agxRender.Color.Red()) elif "gripper_right" in name and "base" not in name: agxOSG.setDiffuseColor(node, agxRender.Color.Blue()) elif "pusher" in name and "base" not in name: agxOSG.setDiffuseColor(node, agxRender.Color.Yellow()) elif "dlo" in name: agxOSG.setDiffuseColor(node, agxRender.Color.Green()) elif "obstacle" in name or "cylinder" in name: agxOSG.setDiffuseColor(node, agxRender.Color.SteelBlue()) elif "bounding_box" in name: agxOSG.setDiffuseColor(node, agxRender.Color.Burlywood()) else: agxOSG.setAlpha(node, 0) logger.info("No color set for {}.".format(name)) if "goal" in name and "base" not in name: agxOSG.setAlpha(node, 0.2) scene_decorator = self.app.getSceneDecorator() light_source_0 = scene_decorator.getLightSource(agxOSG.SceneDecorator.LIGHT0) light_source_0.setPosition(self.light_pose['light_position']) light_source_0.setDirection(self.light_pose['light_direction']) scene_decorator.setEnableLogo(False) scene_decorator.setBackgroundColor(agxRender.Color(1.0, 1.0, 1.0, 1.0))
def add_rendering(sim): camera_distance = 0.5 light_pos = agx.Vec4(CYLINDER_LENGTH / 2, -camera_distance, camera_distance, 1.) light_dir = agx.Vec3(0., 0., -1.) app = agxOSG.ExampleApplication(sim) app.setAutoStepping(False) root = app.getRoot() rbs = sim.getRigidBodies() for rb in rbs: name = rb.getName() node = agxOSG.createVisual(rb, root, 2.0) if "ring" in name: agxOSG.setDiffuseColor(node, COLOR_RING) elif "ground" in name: agxOSG.setDiffuseColor(node, COLOR_GROUND) elif "cylinder" in name: agxOSG.setDiffuseColor(node, COLOR_CYLINDER) elif "gripper_right" == name: agxOSG.setDiffuseColor(node, agxRender.Color(0.0, 0.0, 1.0, 1.0)) elif "gripper_left" == name: agxOSG.setDiffuseColor(node, agxRender.Color(1.0, 0.0, 0.0, 1.0)) else: agxOSG.setDiffuseColor(node, agxRender.Color.Beige()) agxOSG.setAlpha(node, 0.2) app.setEnableDebugRenderer(False) app.setEnableOSGRenderer(True) scene_decorator = app.getSceneDecorator() light_source_0 = scene_decorator.getLightSource( agxOSG.SceneDecorator.LIGHT0) light_source_0.setPosition(light_pos) light_source_0.setDirection(light_dir) scene_decorator.setEnableLogo(False) return app