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 _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(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 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_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 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_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): 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
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 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 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)
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 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 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