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 create_wire(density, size, body_1, pos_1, body_2, pos_2): """ Args: density: size: body_1: pos_1: body_2: pos_2: Returns: """ wire_material = agx.Material('wireMaterial') wire_material.getBulkMaterial().setDensity(density) wire = agxWire.Wire(size, WIRE_RESOLUTION, False) wire.setLinearVelocityDamping(0.8) wire.setMaterial(wire_material) wire.add(agxWire.BodyFixedNode(body_1.m_body, pos_1)) wire.add(agxWire.BodyFixedNode(body_2.link1, pos_2)) wire.setEnableCollisions(True) wire.setEnableCollisions(body_1.m_body, False) wire.setEnableCollisions(body_2.link1, False) wire_renderer = agxOSG.WireRenderer(wire, demoutils.root()) return wire, wire_renderer
def __init__(self, num_servos: int = 2): super().__init__() self.servos = [] # type: list[agx.Hinge] self.sensors = [] # type: list[agx.RigidBody] self.material = agx.Material("snake_material_{}".format(self.getUuid().__str__)) self.len = 0 def connect(rb1: agx.RigidBody, rb2: agx.RigidBody): axis = agx.Vec3(0, 0, -1) pos = agx.Vec3(0.0, 0.007, 0) hinge = snakeapp.create_constraint( pos=pos, axis=axis, c=agx.Hinge, rb1=rb1, rb2=rb2) # type: agx.Hinge hinge.setCompliance(1E-12) hinge.getMotor1D().setEnable(True) hinge.getMotor1D().setCompliance(1E-12) hinge.getLock1D().setEnable(False) hinge.getRange1D().setEnable(True) hinge.getRange1D().setRange(-math.pi / 2, math.pi) self.add(hinge) self.servos.append(hinge) def add(part): self.len += part.len self.add(part) last_part = None for i in range(0, num_servos): if i == 0: type_a = TypeA(self.material) type_b = TypeB(self.material) type_b.setPosition(type_a.len, 0, 0) add(type_a) add(type_b) connect(type_a.bottom, type_b.upper) self.sensors.append(type_a.intermediate.sensor) self.sensors.append(type_b.intermediate.sensor) last_part = type_b elif i == num_servos - 1: type_b = last_part # type: TypeB type_c = TypeC(self.material) type_c.setPosition(type_b.getPosition().x() + type_b.len, 0, 0) add(type_c) connect(type_b.bottom, type_c.upper) self.sensors.append(type_c.intermediate.sensor) last_part = type_c else: type_b1 = last_part # type: TypeB type_b2 = TypeB(self.material) type_b2.setPosition(type_b1.getPosition().x() + type_b1.len, 0, 0) add(type_b2) connect(type_b1.bottom, type_b2.upper) self.sensors.append(type_b2.intermediate.sensor) last_part = type_b2 self.num_servos = len(self.servos) self.num_sensors = len(self.sensors)
def make_water(water_density, water_length, water_width, water_height): """ Args: water_density: water_length: water_width: water_height: Returns: """ seafloorImageBuilder.save_new_seafloor_image(width=water_length) water_material = agx.Material("waterMaterial") water_material.getBulkMaterial().setDensity(water_density) water_geometry = agxCollide.Geometry( agxCollide.Box(water_length, water_width / 2, water_height / 2)) water_geometry.setPosition(0, 0, -water_height / 2) seafloor = MakeWater.make_seafloor(vairance=SEAFLOOR_VARIANCE, length=water_length * 2, width=water_width, depth=-water_height) """Surface of water at z = 0.""" water_geometry.setMaterial(water_material) MakeWater.add_color(water_geometry) """ ads visualisation""" agxOSG.createVisual(seafloor, demoutils.root()) print("build water and seafloor") return water_geometry, seafloor
def __init__(self, num_modules: int, pitch_only=False): super().__init__() self.material = agx.Material("snake_material_{}".format( self.getUuid().__str__)) self.modules = [] for i in range(0, num_modules): module = SnakeModule(self.material) module.setPosition(module_len * i, 0, 0) module.setRotation(agx.EulerAngles(math.pi / 2, 0, 0)) if not pitch_only: if i % 2 != 0: module.setRotation(agx.EulerAngles(0, 0, 0)) if i > 0: merged_body = agx.MergedBody() merged_body.add( agx.MergedBodyEmptyEdgeInteraction( self.modules[i - 1].upper, module.bottom)) snakeapp.add(merged_body) self.modules.append(module) self.add(module)
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 setup_material_coff(sim): """ Set up all the materials properties for the simulation """ brick_damping = 1.0/4 brick_youngs = 2.0E9 brick_material = agx.Material("brick_material") brick_material.getSurfaceMaterial().setRoughness(0.5) brick_material.getSurfaceMaterial().setViscosity(1.e-15) brick_bulk = brick_material.getBulkMaterial() brick_bulk.setDamping(brick_damping) brick_bulk.setYoungsModulus(brick_youngs) gripper_material = agx.Material("gripper_material") gripper_material.getSurfaceMaterial().setRoughness(50) gripper_bulk = gripper_material.getBulkMaterial() gripper_bulk.setDamping(brick_damping) gripper_bulk.setYoungsModulus(brick_youngs) table_material = agx.Material("table_material") table_material.getSurfaceMaterial().setRoughness(10) table_bulk = table_material.getBulkMaterial() table_bulk.setDamping(brick_damping * 100) table_bulk.setYoungsModulus(brick_youngs * 100) friction_model = agx.IterativeProjectedConeFriction() friction_model.setSolveType(agx.FrictionModel.SPLIT) cm_brick_2_brick = sim.getMaterialManager().getOrCreateContactMaterial(brick_material, brick_material) cm_brick_2_brick.setFrictionModel(friction_model) cm_brick_2_brick.setRestitution(0.5) cm_brick_2_table = sim.getMaterialManager().getOrCreateContactMaterial(brick_material, table_material) cm_brick_2_table.setFrictionModel(friction_model) cm_brick_2_table.setRestitution(0.5) cm_gripper_2_brick = sim.getMaterialManager().getOrCreateContactMaterial(gripper_material, brick_material) friction_model_gripper = agx.IterativeProjectedConeFriction() friction_model_gripper.setSolveType(agx.FrictionModel.DIRECT) #Gripper may slip with inaccurate SPLIT solver cm_gripper_2_brick.setFrictionModel(friction_model_gripper) cm_gripper_2_brick.setRestitution(0.1) return table_material, gripper_material, brick_material
def createPond(sim, root): water_material = agx.Material("waterMaterial") water_material.getBulkMaterial().setDensity(1025) water = agxCollide.Geometry(agxCollide.Box(30, 50, 5)) water.setMaterial(water_material) water.setPosition(agxVec([10,-8,45])) sim.add(water) controller = agxModel.WindAndWaterController() controller.addWater(water) create_water_visual(water, root) sim.add(controller)
def __init__(self, length: float = 0.5, angle: float = 20): super().__init__() self._slope_angle = math.radians(angle) self.material = agx.Material("obstacle_material_{}".format(self.getUuid().__str__)) def add_floor(): h = 0.1 floor = agxCollide.Geometry(agxCollide.Box(2.5, 0.5, h), agx.AffineMatrix4x4.translate(0, 0, -h)) snakeapp.create_visual(floor, diffuse_color=agxRender.Color.Green()) self.add(floor) add_floor() def add_slope(): half_extents = agx.Vec3(length, 0.1, 0.005) slope = agxCollide.Geometry(agxCollide.Box(half_extents), agx.AffineMatrix4x4.translate(half_extents.x(), 0, -half_extents.z())) slope.setRotation(agx.EulerAngles(0, -self._slope_angle, 0)) snakeapp.create_visual(slope, diffuse_color=agxRender.Color.Red()) slope.setMaterial(self.material) self.add(slope) return slope self.slope = add_slope() def add_box(): height = math.sin(self._slope_angle) * length half_extents = agx.Vec3(0.5, 0.1, height) box = agxCollide.Geometry(agxCollide.Box(half_extents), agx.AffineMatrix4x4.translate(0, 0, half_extents.z())) box.setParentFrame(self.slope.getParentFrame()) box.setLocalPosition(0.5 + math.cos(self._slope_angle), 0, 0) snakeapp.create_visual(box, diffuse_color=agxRender.Color.Yellow()) box.setMaterial(self.material) self.add(box) add_box()
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 __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 build_simulation(): # Instantiate a simulation sim = agxSDK.Simulation() # By default the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that # too by creating an agx.PointGravityField for example). # AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen) if not GRAVITY: logger.info("Gravity off.") g = agx.Vec3(0, 0, 0) # remove gravity sim.setUniformGravity(g) # Get current delta-t (timestep) that is used in the simulation? dt = sim.getTimeStep() logger.debug("default dt = {}".format(dt)) # Change the timestep sim.setTimeStep(TIMESTEP) # Confirm timestep changed dt = sim.getTimeStep() logger.debug("new dt = {}".format(dt)) # Define materials material_hard = agx.Material("Aluminum") material_hard_bulk = material_hard.getBulkMaterial() material_hard_bulk.setPoissonsRatio(ALUMINUM_POISSON_RATIO) material_hard_bulk.setYoungsModulus(ALUMINUM_YOUNG_MODULUS) # Create gripper create_gripper_peg_in_hole(sim=sim, name="gripper", material=material_hard, position=agx.Vec3(0.0, 0.0, 0.35), geometry_transform=agx.AffineMatrix4x4(), joint_ranges=JOINT_RANGES, force_ranges=FORCE_RANGES) # Create hollow cylinde with hole scaling_cylinder = agx.Matrix3x3(agx.Vec3(0.0275)) hullMesh = agxUtil.createTrimeshFromWavefrontOBJ(MESH_HOLLOW_CYLINDER_FILE, 0, scaling_cylinder) hullGeom = agxCollide.Geometry( hullMesh, agx.AffineMatrix4x4.rotate(agx.Vec3(0, 1, 0), agx.Vec3(0, 0, 1))) hollow_cylinder = agx.RigidBody("hollow_cylinder") hollow_cylinder.add(hullGeom) hollow_cylinder.setMotionControl(agx.RigidBody.STATIC) hullGeom.setMaterial(material_hard) sim.add(hollow_cylinder) # Create rope and set name + properties peg = agxCable.Cable(RADIUS, RESOLUTION) peg.setName("DLO") material_peg = peg.getMaterial() peg_material = material_peg.getBulkMaterial() peg_material.setPoissonsRatio(PEG_POISSON_RATIO) properties = peg.getCableProperties() properties.setYoungsModulus(YOUNG_MODULUS_BEND, agxCable.BEND) properties.setYoungsModulus(YOUNG_MODULUS_TWIST, agxCable.TWIST) properties.setYoungsModulus(YOUNG_MODULUS_STRETCH, agxCable.STRETCH) # Add connection between cable and gripper tf_0 = agx.AffineMatrix4x4() tf_0.setTranslate(0.0, 0, 0.075) peg.add(agxCable.BodyFixedNode(sim.getRigidBody("gripper_body"), tf_0)) peg.add(agxCable.FreeNode(0.0, 0.0, 0.1)) sim.add(peg) segment_iterator = peg.begin() n_segments = peg.getNumSegments() for i in range(n_segments): if not segment_iterator.isEnd(): seg = segment_iterator.getRigidBody() seg.setAngularVelocityDamping(1e3) segment_iterator.inc() # Try to initialize rope report = peg.tryInitialize() if report.successful(): print("Successful rope initialization.") else: print(report.getActualError()) # Add rope to simulation sim.add(peg) # Set rope material material_peg = peg.getMaterial() material_peg.setName("rope_material") contactMaterial = sim.getMaterialManager().getOrCreateContactMaterial( material_hard, material_peg) contactMaterial.setYoungsModulus(1e12) fm = agx.IterativeProjectedConeFriction() fm.setSolveType(agx.FrictionModel.DIRECT) contactMaterial.setFrictionModel(fm) # Add keyboard listener motor_x = sim.getConstraint1DOF("gripper_joint_base_x").getMotor1D() motor_y = sim.getConstraint1DOF("gripper_joint_base_y").getMotor1D() motor_z = sim.getConstraint1DOF("gripper_joint_base_z").getMotor1D() motor_rot_y = sim.getConstraint1DOF("gripper_joint_rot_y").getMotor1D() key_motor_map = { agxSDK.GuiEventListener.KEY_Up: (motor_y, 0.5), agxSDK.GuiEventListener.KEY_Down: (motor_y, -0.5), agxSDK.GuiEventListener.KEY_Right: (motor_x, 0.5), agxSDK.GuiEventListener.KEY_Left: (motor_x, -0.5), 65365: (motor_z, 0.5), 65366: (motor_z, -0.5), 120: (motor_rot_y, 5), 121: (motor_rot_y, -5) } sim.add(KeyboardMotorHandler(key_motor_map)) rbs = peg.getRigidBodies() for i in range(len(rbs)): rbs[i].setName('dlo_' + str(i + 1)) return sim
def build_simulation(goal=False, rope=True): """Builds simulations for both start and goal configurations :param bool goal: toggles between simulation definition of start and goal configurations :param bool rope: add rope to the scene or not :return agxSDK.Simulation: simulation object """ assembly_name = "start_" goal_string = "" if goal: assembly_name = "goal_" goal_string = "_goal" # Instantiate a simulation sim = agxSDK.Simulation() # By default, the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that # too by creating an agx.PointGravityField for example). # AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen) if not GRAVITY: logger.info("Gravity off.") g = agx.Vec3(0, 0, 0) # remove gravity sim.setUniformGravity(g) # Get current delta-t (timestep) that is used in the simulation? dt = sim.getTimeStep() logger.debug("default dt = {}".format(dt)) # Change the timestep sim.setTimeStep(TIMESTEP) # Confirm timestep changed dt = sim.getTimeStep() logger.debug("new dt = {}".format(dt)) # Create a new empty Assembly scene = agxSDK.Assembly() scene.setName(assembly_name + "assembly") # Add start assembly to simulation sim.add(scene) # Define materials material_ground = agx.Material("Aluminum") bulk_material = material_ground.getBulkMaterial() bulk_material.setPoissonsRatio(ALUMINUM_POISSON_RATIO) bulk_material.setYoungsModulus(ALUMINUM_YOUNG_MODULUS) surface_material = material_ground.getSurfaceMaterial() surface_material.setRoughness(GROUND_ROUGHNESS) surface_material.setAdhesion(GROUND_ADHESION, GROUND_ADHESION_OVERLAP) material_pusher = agx.Material("Aluminum") bulk_material = material_pusher.getBulkMaterial() bulk_material.setPoissonsRatio(ALUMINUM_POISSON_RATIO) bulk_material.setYoungsModulus(ALUMINUM_YOUNG_MODULUS) surface_material = material_pusher.getSurfaceMaterial() surface_material.setRoughness(PUSHER_ROUGHNESS) surface_material.setAdhesion(PUSHER_ADHESION, PUSHER_ADHESION_OVERLAP) # Create a ground plane and bounding box to prevent falls ground = create_body(name="ground" + goal_string, shape=agxCollide.Box(GROUND_LENGTH_X, GROUND_LENGTH_Y, GROUND_WIDTH), position=agx.Vec3(0, 0, -GROUND_WIDTH / 2), motion_control=agx.RigidBody.STATIC, material=material_ground) scene.add(ground) bounding_box = create_body(name="bounding_box_1" + goal_string, shape=agxCollide.Box(GROUND_LENGTH_X, GROUND_WIDTH, RADIUS * 4), position=agx.Vec3(0, GROUND_LENGTH_Y - GROUND_WIDTH, RADIUS * 4 - GROUND_WIDTH), motion_control=agx.RigidBody.STATIC, material=material_ground) scene.add(bounding_box) bounding_box = create_body(name="bounding_box_2" + goal_string, shape=agxCollide.Box(GROUND_LENGTH_X, GROUND_WIDTH, RADIUS * 4), position=agx.Vec3(0, - GROUND_LENGTH_Y + GROUND_WIDTH, RADIUS * 4 - GROUND_WIDTH), motion_control=agx.RigidBody.STATIC, material=material_ground) scene.add(bounding_box) bounding_box = create_body(name="bounding_box_3" + goal_string, shape=agxCollide.Box(GROUND_WIDTH, GROUND_LENGTH_Y, RADIUS * 4), position=agx.Vec3(GROUND_LENGTH_X - GROUND_WIDTH, 0, RADIUS * 4 - GROUND_WIDTH), motion_control=agx.RigidBody.STATIC, material=material_ground) scene.add(bounding_box) bounding_box = create_body(name="bounding_box_4" + goal_string, shape=agxCollide.Box(GROUND_WIDTH, GROUND_LENGTH_Y, RADIUS * 4), position=agx.Vec3(- GROUND_LENGTH_X + GROUND_WIDTH, 0, RADIUS * 4 - GROUND_WIDTH), motion_control=agx.RigidBody.STATIC, material=material_ground) scene.add(bounding_box) if rope: # Create rope rope = agxCable.Cable(RADIUS, RESOLUTION) rope.add(agxCable.FreeNode(GROUND_LENGTH_X / 2 - RADIUS * 2, GROUND_LENGTH_Y / 2 - RADIUS * 2, RADIUS * 2)) rope.add(agxCable.FreeNode(GROUND_LENGTH_X / 2 - RADIUS * 2, GROUND_LENGTH_Y / 2 - LENGTH - RADIUS * 2, RADIUS * 2)) # Set rope name and properties rope.setName("DLO" + goal_string) properties = rope.getCableProperties() properties.setYoungsModulus(YOUNG_MODULUS_BEND, agxCable.BEND) properties.setYoungsModulus(YOUNG_MODULUS_TWIST, agxCable.TWIST) properties.setYoungsModulus(YOUNG_MODULUS_STRETCH, agxCable.STRETCH) # Try to initialize rope report = rope.tryInitialize() if report.successful(): print("Successful rope initialization.") else: print(report.getActualError()) # Add rope to simulation scene.add(rope) rbs = rope.getRigidBodies() for i in range(len(rbs)): rbs[i].setName('dlo_' + str(i+1) + goal_string) # Set rope material material_rope = rope.getMaterial() material_rope.setName("rope_material") bulk_material = material_rope.getBulkMaterial() bulk_material.setDensity(ROPE_DENSITY) surface_material = material_rope.getSurfaceMaterial() surface_material.setRoughness(ROPE_ROUGHNESS) surface_material.setAdhesion(ROPE_ADHESION, 0) # Check mass rope_mass = rope.getMass() print("Rope mass: {}".format(rope_mass)) # Create contact materials contact_material_ground_rope = sim.getMaterialManager().getOrCreateContactMaterial(material_ground, material_rope) contact_material_pusher_rope = sim.getMaterialManager().getOrCreateContactMaterial(material_pusher, material_rope) contact_material_ground_rope.setUseContactAreaApproach(True) sim.add(contact_material_ground_rope) sim.add(contact_material_pusher_rope) rotation_cylinder = agx.OrthoMatrix3x3() rotation_cylinder.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.Z_AXIS()) pusher = create_body(name="pusher" + goal_string, shape=agxCollide.Cylinder(PUSHER_RADIUS, PUSHER_HEIGHT), position=agx.Vec3(0.0, 0.0, PUSHER_HEIGHT / 2), rotation=rotation_cylinder, motion_control=agx.RigidBody.DYNAMICS, material=material_pusher) scene.add(pusher) # Create base for pusher motors prismatic_base = create_locked_prismatic_base("pusher" + goal_string, pusher.getRigidBody("pusher" + goal_string), position_ranges=[(-GROUND_LENGTH_X, GROUND_LENGTH_X), (-GROUND_LENGTH_Y, GROUND_LENGTH_Y), (0., 3 * RADIUS)], motor_ranges=[(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE), (-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE), (-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE)]) scene.add(prismatic_base) return sim
def build_material(name, density): material = agx.Material(name) material.getBulkMaterial().setDensity(density) return material
def __init__(self, num_robots: int = 1): super().__init__() self.jointCon = [] # type: list[agx.Hinge] self.forceFeedback = [] # type: list[agx.RigidBody] self.len = 0 self.material = agx.Material("robot_material{}".format( self.getUuid().__str__)) def connect(rb1: agx.RigidBody, rb2: agx.RigidBody): axis = agx.Vec3(0, 0, 0) pos = agx.Vec3( 0, 0, 0, ) hinge = oneLegRobotApp.create_constraint( pos=pos, axis=axis, c=agx.Hinge, rb1=rb1, rb2=rb2) # type: agx.Hinge hinge.setCompliance(1E-12) hinge.getMotor1D().setEnable(True) hinge.getMotor1D().setCompliance(1E-12) hinge.getLock1D().setEnable(False) hinge.getRange1D().setEnable(True) hinge.getRange1D().setRange(-math.pi / 2, math.pi) self.add(hinge) self.jointCon.append(hinge) # def add(part): # self.len += part.len # self.add(part) # # last_part = None #testRobotLeg = OneLegRobotAssembly(self.material) self.floor = OneLegRobotAssembly(self.material).create_floor() self.first_joint_geometry_aft = OneLegRobotAssembly( self.material).create_section(20, -180 / 2, 440, 0, 0, math.pi / 2, first_joint_shape_aft) self.second_joint_geometry_aft = OneLegRobotAssembly( self.material).create_section(0, -180 / 2, 320, 0, 0, math.pi / 2, second_joint_shape_aft) self.first_joint_geometry_fwd = OneLegRobotAssembly( self.material).create_section(0, 180 / 2, 440, 0, 0, math.pi / 2, first_joint_shape_fwd) self.second_joint_geometry_fwd = OneLegRobotAssembly( self.material).create_section(-20, 180 / 2, 320, 0, 0, math.pi / 2, second_joint_shape_fwd) # Connect joints connect(self.first_joint_geometry_aft, self.second_joint_geometry_aft) connect(self.first_joint_geometry_fwd, self.second_joint_geometry_fwd) # Build the robot oneLegRobotApp.add(self.floor) self.second_joint_geometry_aft.setParentFrame( self.first_joint_geometry_aft.getFrame()) self.second_joint_geometry_fwd.setParentFrame( self.first_joint_geometry_fwd.getFrame()) oneLegRobotApp.add(self.first_joint_geometry_aft) oneLegRobotApp.add(self.second_joint_geometry_aft) oneLegRobotApp.add(self.first_joint_geometry_fwd) oneLegRobotApp.add(self.second_joint_geometry_fwd) # Arrange camera to be centered on floor oneLegRobotApp.init_camera(eye=agx.Vec3(800, 0, 800), center=self.floor.getPosition())
def __init__(self, **kwargs): """ Construct given Arguments: local_transform: agx.AffineMatrix4x4 - [Optional] local transform so that track y axis is the rotation axis. """ super().__init__() self.m_sprocket_hinge = kwargs[ 'sprocket_hinge' ] track_material = kwargs.get('track_material', agx.Material("DefaultTrackMaterial")) sprocket_body = kwargs[ 'sprocket_body' ] self.add(sprocket_body) sprocket_body_info = RigidBodyInfo( sprocket_body ) # We will assume all sprockets/Rollers/Idlers has the same material! self.m_roller_material = None # Get the material from the first geometry which has collision enabled for g in sprocket_body_info.geometries: if (g.collisions_enabled): self.m_roller_material = g.instance.getMaterial() break assert(self.m_roller_material) idler_body = kwargs['idler_body'] self.add(idler_body) roller_bodies = kwargs['roller_bodies'] local_transform = kwargs.get( 'local_transform', agx.AffineMatrix4x4() ) track_settings = kwargs.get( 'track_settings') self.m_track = agxVehicle.Track( track_settings.track_parameters.number_of_shoes, track_settings.track_parameters.width, track_settings.track_parameters.thickness, track_settings.track_parameters.initial_offset ) self.m_track.setMaterial(track_material) radius = Track.find_radius( sprocket_body ) sprocket = agxVehicle.TrackWheel( agxVehicle.TrackWheel.SPROCKET, radius, sprocket_body, local_transform ) self.m_track.add( sprocket ) radius = Track.find_radius( idler_body ) idler = agxVehicle.TrackWheel( agxVehicle.TrackWheel.IDLER, radius, idler_body, local_transform ) self.m_track.add( idler ) idler.getLocalFrame().setLocalMatrix( agx.AffineMatrix4x4.rotate( agx.Vec3.Y_AXIS(), agx.Vec3.X_AXIS() ) ) for b in roller_bodies: self.add(b) radius = Track.find_radius( b ) roller = agxVehicle.TrackWheel( agxVehicle.TrackWheel.ROLLER, radius, b, local_transform ) roller.setEnableProperty( agxVehicle.TrackWheel.SPLIT_SEGMENTS, True ) self.m_track.add( roller ) track_merge_parameters_settings = track_settings.track_parameters.internal_merge mergeProperties = self.m_track.getInternalMergeProperties() mergeProperties.setEnableMerge( track_merge_parameters_settings.enabled) mergeProperties.setNumNodesPerMergeSegment( track_merge_parameters_settings.number_of_nodes_per_segment ) mergeProperties.setEnableLockToReachMergeCondition( track_merge_parameters_settings.use_lock ) mergeProperties.setLockToReachMergeConditionCompliance( track_merge_parameters_settings.lock_compliance ) mergeProperties.setLockToReachMergeConditionDamping( track_merge_parameters_settings.lock_damping ) mergeProperties.setMaxAngleMergeCondition( track_merge_parameters_settings.merge_angle ) mergeProperties.setContactReduction( track_merge_parameters_settings.contact_reduction_level ) track_properties_settings = track_settings.track_parameters.properties properties = self.m_track.getProperties() properties.setHingeCompliance( track_properties_settings.compliance ) properties.setHingeDamping( track_properties_settings.damping ) properties.setEnableHingeRange( track_properties_settings.range_enable ) properties.setHingeRangeRange( track_properties_settings.range_range.lower, track_properties_settings.range_range.upper) properties.setEnableOnInitializeMergeNodesToWheels( track_properties_settings.on_initialize_merge_nodes_to_wheels ) properties.setEnableOnInitializeTransformNodesToWheels( track_properties_settings.on_initialize_transform_nodes_to_wheels ) properties.setTransformNodesToWheelsOverlap( track_properties_settings.target_node_wheel_overlap ) properties.setNodesToWheelsMergeThreshold( track_properties_settings.node_wheel_merge_threshold ) properties.setNodesToWheelsSplitThreshold( track_properties_settings.node_wheel_split_threshold) properties.setNumNodesIncludedInAverageDirection( track_properties_settings.num_nodes_in_averge_direction) properties.setMinStabilizingHingeNormalForce( track_properties_settings.min_stabilizing_normal_force) properties.setStabilizingHingeFrictionParameter( track_properties_settings.stabilizing_friction_parameter) self.add(self.m_track) self.m_track.initialize() print( "Loading shoe visual: ", track_settings.shoe_visual_filename) shoe_visual_data = agxOSG.readNodeFile( track_settings.shoe_visual_filename, False ) assert( shoe_visual_data ) print( " ...done." ) shoe_visual_transform = agxOSG.Transform() shoe_visual_transform.addChild( shoe_visual_data ) nodes = self.m_track.nodes() it = nodes.begin() while it != nodes.end(): node = it.get() gn = agxOSG.GeometryNode( node.getRigidBody().getGeometries()[ 0 ] ) gn.addChild( shoe_visual_transform ) root().addChild( gn ) it.inc()
def build_simulation(): # Instantiate a simulation sim = agxSDK.Simulation() # By default the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that # too by creating an agx.PointGravityField for example). # AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen) if not GRAVITY: logger.info("Gravity off.") g = agx.Vec3(0, 0, 0) # remove gravity sim.setUniformGravity(g) # Get current delta-t (timestep) that is used in the simulation? dt = sim.getTimeStep() logger.debug("default dt = {}".format(dt)) # Change the timestep sim.setTimeStep(TIMESTEP) # Confirm timestep changed dt = sim.getTimeStep() logger.debug("new dt = {}".format(dt)) # Define materials material_hard = agx.Material("Aluminum") material_hard_bulk = material_hard.getBulkMaterial() material_hard_bulk.setPoissonsRatio(ALUMINUM_POISSON_RATIO) material_hard_bulk.setYoungsModulus(ALUMINUM_YOUNG_MODULUS) # Create box with pocket side_length = 0.15 thickness_outer_wall = 0.01 body = create_body(name="ground", shape=agxCollide.Box(side_length, side_length, 0.01), position=agx.Vec3(0, 0, -0.005), motion_control=agx.RigidBody.STATIC, material=material_hard) sim.add(body) body = create_body(name="walls", shape=agxCollide.Box(thickness_outer_wall, side_length, 0.04), position=agx.Vec3(side_length + thickness_outer_wall, 0, 0.0), motion_control=agx.RigidBody.STATIC, material=material_hard) sim.add(body) body = create_body(name="walls", shape=agxCollide.Box(thickness_outer_wall, side_length, 0.04), position=agx.Vec3(-(side_length + thickness_outer_wall), 0, 0.0), motion_control=agx.RigidBody.STATIC, material=material_hard) sim.add(body) body = create_body(name="walls", shape=agxCollide.Box(side_length + 2 * thickness_outer_wall, thickness_outer_wall, 0.04), position=agx.Vec3(0, -(side_length + thickness_outer_wall), 0.0), motion_control=agx.RigidBody.STATIC, material=material_hard) sim.add(body) body = create_body(name="walls", shape=agxCollide.Box(side_length + 2 * thickness_outer_wall, thickness_outer_wall, 0.04), position=agx.Vec3(0, side_length + thickness_outer_wall, 0.0), motion_control=agx.RigidBody.STATIC, material=material_hard) sim.add(body) # Create gripper 0 gripper_0 = create_body(name="gripper_0", shape=agxCollide.Sphere(0.005), position=agx.Vec3(-(LENGTH/2), OFFSET_Y, 0.0025), motion_control=agx.RigidBody.DYNAMICS, material=material_hard) gripper_0.getRigidBody("gripper_0").getGeometry("gripper_0").setEnableCollisions(False) sim.add(gripper_0) # Create base for gripper motors prismatic_base_0 = create_locked_prismatic_base("gripper_0", gripper_0.getRigidBody("gripper_0"), position_ranges=[(-side_length*2, side_length*2), (-side_length*2, side_length*2), (-0.1, 0.01)], motor_ranges=[(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE), (-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE), (-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE)], lock_status=[False, False, False]) sim.add(prismatic_base_0) # Create gripper gripper_1 = create_body(name="gripper_1", shape=agxCollide.Sphere(0.005), position=agx.Vec3((LENGTH/2), OFFSET_Y, 0.0025), motion_control=agx.RigidBody.DYNAMICS, material=material_hard) gripper_1.getRigidBody("gripper_1").getGeometry("gripper_1").setEnableCollisions(False) sim.add(gripper_1) # Create base for gripper motors prismatic_base_0 = create_locked_prismatic_base("gripper_1", gripper_1.getRigidBody("gripper_1"), position_ranges=[(-side_length*2, side_length*2), (-side_length*2, side_length*2), (-0.1, 0.01)], motor_ranges=[(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE), (-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE), (-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE)], lock_status=[False, False, False]) sim.add(prismatic_base_0) # Create goal obstacle goal_obstacle = create_body(name="obstacle_goal", shape=agxCollide.Cylinder(2 * R_GOAL_OBSTACLE, 0.1), position=agx.Vec3(0.0,0.-0.075, 0.005), motion_control=agx.RigidBody.STATIC, material=material_hard) sim.add(goal_obstacle) rotation_cylinder = agx.OrthoMatrix3x3() rotation_cylinder.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.Z_AXIS()) goal_obstacle.setRotation(rotation_cylinder) # Create obstacles obs_pos = OBSTACLE_POSITIONS for i in range(0, len(obs_pos)): obstacle = create_body(name="obstacle", shape=agxCollide.Box(0.01, 0.015, 0.05), position=agx.Vec3(obs_pos[i][0], obs_pos[i][1], 0.005), motion_control=agx.RigidBody.STATIC, material=material_hard) sim.add(obstacle) # Create rope and set name + properties dlo = agxCable.Cable(RADIUS, RESOLUTION) dlo.setName("DLO") material_rubber_band= dlo.getMaterial() rubber_band_material = material_rubber_band.getBulkMaterial() rubber_band_material.setPoissonsRatio(PEG_POISSON_RATIO) properties = dlo.getCableProperties() properties.setYoungsModulus(YOUNG_MODULUS_BEND, agxCable.BEND) properties.setYoungsModulus(YOUNG_MODULUS_TWIST, agxCable.TWIST) properties.setYoungsModulus(YOUNG_MODULUS_STRETCH, agxCable.STRETCH) dlo.add(agxCable.FreeNode(gripper_0.getRigidBody("gripper_0").getPosition())) dlo.add(agxCable.FreeNode(gripper_1.getRigidBody("gripper_1").getPosition())) # # hf = agx.HingeFrame() # hf.setCenter(gripper_0.getRigidBody("gripper_0").getPosition()) # hf.setAxis(agx.Vec3(0,1,0)) # hinge_0 = agx.Hinge(hf, base_z, rot_y) # agx.Hinge() # dlo.add(agxCable.BodyFixedNode(gripper_0.getRigidBody("gripper_0"), agx.Vec3())) # dlo.add(agxCable.BodyFixedNode(gripper_1.getRigidBody("gripper_1"), agx.Vec3())) # Set angular damping for segments sim.add(dlo) segment_iterator = dlo.begin() n_segments = dlo.getNumSegments() segments = [] for i in range(n_segments): if not segment_iterator.isEnd(): seg = segment_iterator.getRigidBody() segments.append(seg) seg.setAngularVelocityDamping(2e4) segment_iterator.inc() mass_props = seg.getMassProperties() mass_props.setMass(1.25*mass_props.getMass()) s0 = segments[0] s1 = segments[-1] h0 = agx.HingeFrame() h0.setCenter(gripper_0.getRigidBody("gripper_0").getPosition()) h0.setAxis(agx.Vec3(0,0,1)) l0 = agx.Hinge(h0, s0, gripper_0.getRigidBody("gripper_0") ) sim.add(l0) h1 = agx.HingeFrame() h1.setCenter(gripper_1.getRigidBody("gripper_1").getPosition()) h1.setAxis(agx.Vec3(0,0,1)) l1 = agx.Hinge(h1, s1, gripper_1.getRigidBody("gripper_1") ) sim.add(l1) # f0 = agx.Frame() # f1 = agx.Frame() # l0 = agx.LockJoint(s0, f0, gripper_0.getRigidBody("gripper_0"), f1) # l1 = agx.LockJoint(s1, f0, gripper_1.getRigidBody("gripper_1"), f1) # sim.add(l0) # sim.add(l1) # Try to initialize dlo report = dlo.tryInitialize() if report.successful(): print("Successful dlo initialization.") else: print(report.getActualError()) # Add rope to simulation sim.add(dlo) # Set rope material material_rubber_band = dlo.getMaterial() material_rubber_band.setName("rope_material") contactMaterial = sim.getMaterialManager().getOrCreateContactMaterial(material_hard, material_rubber_band) contactMaterial.setYoungsModulus(1e12) fm = agx.IterativeProjectedConeFriction() fm.setSolveType(agx.FrictionModel.DIRECT) contactMaterial.setFrictionModel(fm) # Add keyboard listener motor_x_0 = sim.getConstraint1DOF("gripper_0_joint_base_x").getMotor1D() motor_y_0 = sim.getConstraint1DOF("gripper_0_joint_base_y").getMotor1D() motor_x_1 = sim.getConstraint1DOF("gripper_1_joint_base_x").getMotor1D() motor_y_1 = sim.getConstraint1DOF("gripper_1_joint_base_y").getMotor1D() key_motor_map = {agxSDK.GuiEventListener.KEY_Up: (motor_y_0, 0.5), agxSDK.GuiEventListener.KEY_Down: (motor_y_0, -0.5), agxSDK.GuiEventListener.KEY_Right: (motor_x_0, 0.5), agxSDK.GuiEventListener.KEY_Left: (motor_x_0, -0.5), 120: (motor_x_1, 0.5), 60: (motor_x_1, -0.5), 97: (motor_y_1, 0.5), 121: (motor_y_1, -0.5)} sim.add(KeyboardMotorHandler(key_motor_map)) rbs = dlo.getRigidBodies() for i in range(len(rbs)): rbs[i].setName('dlo_' + str(i+1)) return sim
def _initialize(self, data: SimulationFile, **kwargs): self.m_body_chassie = data.bodies[ 'ChassieBody' ] self.m_body_under_carriage = data.bodies[ 'UnderCarriageBody' ] self.m_body_bucket = data.bodies[ 'Bucket' ] self.m_body_sprockets = [data.bodies['LeftFrontSprocket'], data.bodies['RightFrontSprocket']] self.m_body_idlers = [data.bodies['LeftRearIdler'], data.bodies['RightRearIdler']] self.m_sprocket_hinges = [data.constraints['LeftSprocketHinge'].asHinge(), data.constraints['RightSprocketHinge'].asHinge()] self.m_left_roller_bodies = [] self.m_right_roller_bodies = [] for i in range(1,10): name = 'RightRoller'+str(i) if name in data.bodies: self.m_right_roller_bodies.append(data.bodies[name]) name = 'LeftRoller'+str(i) if name in data.bodies: self.m_left_roller_bodies.append(data.bodies[name]) self.m_under_carriage_observer = data.observers['UnderCarriageObserver'] # Creating tracks: [left,right] # The local transform makes sure the y axes are pointing out from # the track center positions. local_track_transform = agx.AffineMatrix4x4.rotate( agx.Vec3.Z_AXIS(), agx.Vec3.Y_AXIS() ) track_settings = kwargs.get( 'track_settings', self.default_track_settings() ) track_material = agx.Material("TrackMaterial") self.m_tracks = [ Track( track_settings = track_settings, sprocket_hinge = self.sprocket_hinge(self.Location.LEFT), sprocket_body = self.sprocket_body(self.Location.LEFT), idler_body = self.idler_body(self.Location.LEFT), roller_bodies = self.roller_bodies(self.Location.LEFT), local_transform = local_track_transform, track_material = track_material ), Track( track_settings = track_settings, sprocket_hinge = self.sprocket_hinge(self.Location.RIGHT), sprocket_body = self.sprocket_body(self.Location.RIGHT), idler_body = self.idler_body(self.Location.RIGHT), roller_bodies = self.roller_bodies(self.Location.RIGHT), local_transform = local_track_transform, track_material = track_material ) ] for t in self.m_tracks: self.add(t) t.track_material = track_material self.configure_track_roller_contact_materials(self.m_tracks[0].track_material, self.m_tracks[0].roller_material) self.initialize_bucket_data(data) ################################################################################### # Drivetrain # ################################################################################### ################################################################################### driveline_settings = kwargs.get( 'driveline_settings', self.default_driveline_settings() ) def setup_sprocket_hinge(hinge, driveline_settings): hinge.getMotor1D().setForceRange( agx.RangeReal(driveline_settings.max_torque)) hinge.getMotor1D().setCompliance( driveline_settings.compliance ) hinge.getMotor1D().setSpeed( driveline_settings.speed ) hinge.getMotor1D().setLockedAtZeroSpeed( driveline_settings.lockedAtZeroSpeed ) setup_sprocket_hinge(self.sprocket_hinge(self.Location.LEFT), driveline_settings.hinge) setup_sprocket_hinge(self.sprocket_hinge(self.Location.RIGHT), driveline_settings.hinge) # Get the constraints for controlling the cabin/arm/boom self.m_boom_prismatics = [data.constraints['BoomPrismatic1'].asPrismatic(), data.constraints['BoomPrismatic2'].asPrismatic()] self.m_bucket_prismatic = data.constraints['BucketPrismatic'].asPrismatic() self.m_arm_prismatic = data.constraints['ArmPrismatic'].asPrismatic() self.m_cabin_hinge = data.constraints['CabinHinge'].asHinge() self.m_keyboard_controls = None self.m_gamepad_controls = None self.keyboard_controls = kwargs.get( 'keyboard_controls', None ) self.gamepad_controls = kwargs.get( 'gamepad_controls', None )
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 create_rigid_body_material_from_particle_material(pmat): mat = agx.Material("rb_%s" % pmat.getName()) mat.getBulkMaterial().setDensity(pmat.getBulkMaterial().getDensity()) # mat.getBulkMaterial().setYoungsModulus(pmat.getBulkMaterial().getYoungsModulus()) # mat.getBulkMaterial().setPoissonsRatio(pmat.getBulkMaterial().getPoissonsRatio()) return mat
def build_simulation(): # Instantiate a simulation sim = agxSDK.Simulation() # By default the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that # too by creating an agx.PointGravityField for example). # AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen) if not GRAVITY: logger.info("Gravity off.") g = agx.Vec3(0, 0, 0) # remove gravity sim.setUniformGravity(g) # Get current delta-t (timestep) that is used in the simulation? dt = sim.getTimeStep() logger.debug("default dt = {}".format(dt)) # Change the timestep sim.setTimeStep(TIMESTEP) # Confirm timestep changed dt = sim.getTimeStep() logger.debug("new dt = {}".format(dt)) # Create a ground plane for reference ground = create_body(name="ground", shape=agxCollide.Box(CYLINDER_LENGTH * 2, CYLINDER_LENGTH * 2, GROUND_WIDTH), position=agx.Vec3(0, 0, 0), motion_control=agx.RigidBody.STATIC) sim.add(ground) rotation_cylinder = agx.OrthoMatrix3x3() rotation_cylinder.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.Z_AXIS()) material_cylinder = agx.Material("Aluminum") bulk_material_cylinder = material_cylinder.getBulkMaterial() bulk_material_cylinder.setPoissonsRatio(ALUMINUM_POISSON_RATIO) bulk_material_cylinder.setYoungsModulus(ALUMINUM_YOUNG_MODULUS) # Create cylinders bottom_cylinder = create_body( name="bottom_cylinder", shape=agxCollide.Cylinder(CYLINDER_RADIUS, 3 / 4 * CYLINDER_LENGTH), position=agx.Vec3(0, 0, GROUND_WIDTH + (3 / 4 * CYLINDER_LENGTH) / 2), rotation=rotation_cylinder, motion_control=agx.RigidBody.STATIC, material=material_cylinder) sim.add(bottom_cylinder) middle_cylinder = create_body( name="middle_cylinder", shape=agxCollide.Cylinder(CYLINDER_RADIUS - GROOVE_DEPTH, GROOVE_WIDTH), position=agx.Vec3( 0, 0, GROUND_WIDTH + (3 / 4 * CYLINDER_LENGTH) + GROOVE_WIDTH / 2), rotation=rotation_cylinder, motion_control=agx.RigidBody.STATIC, material=material_cylinder) sim.add(middle_cylinder) top_cylinder = create_body( name="top_cylinder", shape=agxCollide.Cylinder(CYLINDER_RADIUS, 1 / 4 * CYLINDER_LENGTH), position=agx.Vec3( 0, 0, GROUND_WIDTH + (3 / 4 * CYLINDER_LENGTH) + GROOVE_WIDTH + (1 / 4 * CYLINDER_LENGTH) / 2), rotation=rotation_cylinder, motion_control=agx.RigidBody.STATIC, material=material_cylinder) sim.add(top_cylinder) material_ring = agx.Material("Rubber") bulk_material_ring = material_ring.getBulkMaterial() bulk_material_ring.setPoissonsRatio(RUBBER_POISSON_RATIO) bulk_material_ring.setYoungsModulus(RUBBER_YOUNG_MODULUS) ring = create_ring(name="ring", radius=RING_RADIUS, element_shape=agxCollide.Capsule( RING_CROSS_SECTION, RING_SEGMENT_LENGTH), num_elements=NUM_RING_ELEMENTS, constraint_type=agx.LockJoint, rotation_shift=math.pi / 2, translation_shift=RING_SEGMENT_LENGTH / 2, compliance=RING_COMPLIANCE, center=agx.Vec3(0, 0, CYLINDER_LENGTH + RING_RADIUS), material=material_ring) sim.add(ring) left_ring_element = sim.getRigidBody(LEFT_ELEMENT) right_ring_element = sim.getRigidBody(RIGHT_ELEMENT) gripper_left = create_body( name="gripper_left", shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER, SIZE_GRIPPER), position=left_ring_element.getPosition(), rotation=agx.OrthoMatrix3x3(left_ring_element.getRotation()), motion_control=agx.RigidBody.DYNAMICS) sim.add(gripper_left) gripper_right = create_body( name="gripper_right", shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER, SIZE_GRIPPER), position=right_ring_element.getPosition(), rotation=agx.OrthoMatrix3x3(right_ring_element.getRotation()), motion_control=agx.RigidBody.DYNAMICS) sim.add(gripper_right) # Disable collisions for grippers gripper_left_body = sim.getRigidBody("gripper_left") gripper_left_body.getGeometry("gripper_left").setEnableCollisions(False) gripper_right_body = sim.getRigidBody("gripper_right") gripper_right_body.getGeometry("gripper_right").setEnableCollisions(False) frame_element = agx.Frame() frame_gripper = agx.Frame() result = agx.Constraint.calculateFramesFromBody( agx.Vec3(RING_CROSS_SECTION, 0, 0), agx.Vec3(1, 0, 0), left_ring_element, frame_element, gripper_left_body, frame_gripper) print(result) lock_joint_left = agx.LockJoint(gripper_left_body, frame_gripper, left_ring_element, frame_element) lock_joint_left.setName('lock_joint_left') lock_joint_left.setCompliance(GRIPPER_COMPLIANCE) sim.add(lock_joint_left) frame_gripper = agx.Frame() result = agx.Constraint.calculateFramesFromBody( agx.Vec3(RING_CROSS_SECTION, 0, 0), agx.Vec3(1, 0, 0), right_ring_element, frame_element, gripper_right_body, frame_gripper) print(result) lock_joint_right = agx.LockJoint(gripper_right_body, frame_gripper, right_ring_element, frame_element) lock_joint_right.setName('lock_joint_right') lock_joint_right.setCompliance(GRIPPER_COMPLIANCE) sim.add(lock_joint_right) # Create contact materials contact_material = sim.getMaterialManager().getOrCreateContactMaterial( material_cylinder, material_ring) contact_material.setYoungsModulus(CONTACT_YOUNG_MODULUS) # Create friction model, DIRECT is more accurate, but slower # fm = agx.IterativeProjectedConeFriction() # fm.setSolveType(agx.FrictionModel.DIRECT) # contact_material.setFrictionModel(fm) # Create bases for gripper motors prismatic_base_left = create_hinge_prismatic_base( "gripper_left", gripper_left_body, position_ranges=[(-CYLINDER_RADIUS, CYLINDER_RADIUS), (-CYLINDER_RADIUS, CYLINDER_RADIUS), (-(CYLINDER_LENGTH + 2 * RING_RADIUS), RING_RADIUS), (-math.pi / 4, math.pi / 4)]) sim.add(prismatic_base_left) prismatic_base_right = create_hinge_prismatic_base( "gripper_right", gripper_right_body, position_ranges=[(-CYLINDER_RADIUS, CYLINDER_RADIUS), (-CYLINDER_RADIUS, CYLINDER_RADIUS), (-CYLINDER_LENGTH - RING_RADIUS, RING_RADIUS), (-math.pi / 4, math.pi / 4)]) sim.add(prismatic_base_right) # Add keyboard listener left_motor_x = sim.getConstraint1DOF( "gripper_left_joint_base_x").getMotor1D() left_motor_y = sim.getConstraint1DOF( "gripper_left_joint_base_y").getMotor1D() left_motor_z = sim.getConstraint1DOF( "gripper_left_joint_base_z").getMotor1D() left_motor_rb = sim.getConstraint1DOF("gripper_left_joint_rb").getMotor1D() right_motor_x = sim.getConstraint1DOF( "gripper_right_joint_base_x").getMotor1D() right_motor_y = sim.getConstraint1DOF( "gripper_right_joint_base_y").getMotor1D() right_motor_z = sim.getConstraint1DOF( "gripper_right_joint_base_z").getMotor1D() right_motor_rb = sim.getConstraint1DOF( "gripper_right_joint_rb").getMotor1D() key_motor_map = { agxSDK.GuiEventListener.KEY_Right: (right_motor_x, 0.1), agxSDK.GuiEventListener.KEY_Left: (right_motor_x, -0.1), agxSDK.GuiEventListener.KEY_Up: (right_motor_y, 0.1), agxSDK.GuiEventListener.KEY_Down: (right_motor_y, -0.1), 65365: (right_motor_z, 0.1), 65366: (right_motor_z, -0.1), 0x64: (left_motor_x, 0.1), 0x61: (left_motor_x, -0.1), 0x32: (left_motor_y, 0.1), # 0x77 W is replaced with 2, due to prior shortcut 0x73: (left_motor_y, -0.1), 0x71: (left_motor_z, 0.1), 0x65: (left_motor_z, -0.1), 0x72: (left_motor_rb, 0.1), # R 0x74: (left_motor_rb, -0.1), # T 0x6f: (right_motor_rb, 0.1), # O 0x70: (right_motor_rb, -0.1) } # P sim.add(KeyboardMotorHandler(key_motor_map)) return sim
def build_simulation(): # Instantiate a simulation sim = agxSDK.Simulation() # By default the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that # too by creating an agx.PointGravityField for example). # AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen) if not GRAVITY: logger.info("Gravity off.") g = agx.Vec3(0, 0, 0) # remove gravity sim.setUniformGravity(g) # Get current delta-t (timestep) that is used in the simulation? dt = sim.getTimeStep() logger.debug("default dt = {}".format(dt)) # Change the timestep sim.setTimeStep(TIMESTEP) # Confirm timestep changed dt = sim.getTimeStep() logger.debug("new dt = {}".format(dt)) # Define materials material_hard = agx.Material("Aluminum") material_hard_bulk = material_hard.getBulkMaterial() material_hard_bulk.setPoissonsRatio(ALUMINUM_POISSON_RATIO) material_hard_bulk.setYoungsModulus(ALUMINUM_YOUNG_MODULUS) # Create a ground plane ground = create_body(name="ground", shape=agxCollide.Box(GROUND_LENGTH_X, GROUND_LENGTH_Y, GROUND_HEIGHT), position=agx.Vec3(0, 0, -0.005), motion_control=agx.RigidBody.STATIC) sim.add(ground) # Creates poles for i, position in enumerate(POLE_POSITION_OFFSETS): create_pole(id=i, sim=sim, position=position, material=material_hard) # Create gripper gripper = create_body(name="gripper", shape=agxCollide.Sphere(0.002), position=agx.Vec3(0.0, 0.0, GRIPPER_HEIGHT + DIAMETER / 2.0), motion_control=agx.RigidBody.DYNAMICS, material=material_hard) gripper.getRigidBody("gripper").getGeometry("gripper").setEnableCollisions( False) sim.add(gripper) # Create base for pusher motors prismatic_base = create_locked_prismatic_base( "gripper", gripper.getRigidBody("gripper"), position_ranges=[(-GRIPPER_MAX_X, GRIPPER_MAX_X), (-GRIPPER_MAX_Y, GRIPPER_MAX_Y), (GRIPPER_MIN_Z, GRIPPER_MAX_Z)], motor_ranges=[(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE), (-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE), (-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE)]) sim.add(prismatic_base) # Create rope and set name + properties rubber_band = agxCable.Cable(RADIUS, RESOLUTION) rubber_band.setName("DLO") material_rubber_band = rubber_band.getMaterial() rubber_band_material = material_rubber_band.getBulkMaterial() rubber_band_material.setPoissonsRatio(PEG_POISSON_RATIO) properties = rubber_band.getCableProperties() properties.setYoungsModulus(YOUNG_MODULUS_BEND, agxCable.BEND) properties.setYoungsModulus(YOUNG_MODULUS_TWIST, agxCable.TWIST) properties.setYoungsModulus(YOUNG_MODULUS_STRETCH, agxCable.STRETCH) # Initialize dlo on circle steps = DLO_CIRCLE_STEPS for a in np.linspace(-np.pi / 2, (3.0 / 2.0) * np.pi - 2 * np.pi / steps, steps): x = np.cos(a) * DIAMETER / 2.0 z = np.sin(a) * DIAMETER / 2.0 rubber_band.add(agxCable.FreeNode(x, 0, GRIPPER_HEIGHT + z)) sim.add(rubber_band) segments_cable = list() cable = agxCable.Cable.find(sim, "DLO") segment_iterator = cable.begin() n_segments = cable.getNumSegments() for i in range(n_segments): if not segment_iterator.isEnd(): seg = segment_iterator.getRigidBody() seg.setAngularVelocityDamping(1e4) mass_props = seg.getMassProperties() mass_props.setMass(1.25 * mass_props.getMass()) segments_cable.append(seg) segment_iterator.inc() # Get segments at ends and middle s0 = segments_cable[0] s1 = segments_cable[int(n_segments / 2)] s2 = segments_cable[-1] # Add ball joint between gripper and rubber band f0 = agx.Frame() f1 = agx.Frame() ball_joint = agx.BallJoint(gripper.getRigidBody("gripper"), f0, s1, f1) sim.add(ball_joint) # Connect ends of rubber band f0 = agx.Frame() f0.setLocalTranslate(0.0, 0.0, -1 * np.pi * DIAMETER / cable.getNumSegments()) f1 = agx.Frame() lock = agx.LockJoint(s0, f0, s2, f1) lock.setCompliance(1.0e-4) sim.add(lock) # Try to initialize dlo report = rubber_band.tryInitialize() if report.successful(): print("Successful dlo initialization.") else: print(report.getActualError()) # Add rope to simulation sim.add(rubber_band) # Set rope material material_rubber_band = rubber_band.getMaterial() material_rubber_band.setName("rope_material") contactMaterial = sim.getMaterialManager().getOrCreateContactMaterial( material_hard, material_rubber_band) contactMaterial.setYoungsModulus(1e12) fm = agx.IterativeProjectedConeFriction() fm.setSolveType(agx.FrictionModel.DIRECT) contactMaterial.setFrictionModel(fm) # Add keyboard listener motor_x = sim.getConstraint1DOF("gripper_joint_base_x").getMotor1D() motor_y = sim.getConstraint1DOF("gripper_joint_base_y").getMotor1D() motor_z = sim.getConstraint1DOF("gripper_joint_base_z").getMotor1D() key_motor_map = { agxSDK.GuiEventListener.KEY_Up: (motor_y, 0.2), agxSDK.GuiEventListener.KEY_Down: (motor_y, -0.2), agxSDK.GuiEventListener.KEY_Right: (motor_x, 0.2), agxSDK.GuiEventListener.KEY_Left: (motor_x, -0.2), 65365: (motor_z, 0.2), 65366: (motor_z, -0.2) } sim.add(KeyboardMotorHandler(key_motor_map)) rbs = rubber_band.getRigidBodies() for i in range(len(rbs)): rbs[i].setName('dlo_' + str(i + 1)) return sim
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 build_simulation(goal=False): """Builds simulations for both start and goal configurations :param bool goal: toggles between simulation definition of start and goal configurations :return agxSDK.Simulation: simulation object """ assembly_name = "start_" goal_string = "" if goal: assembly_name = "goal_" goal_string = "_goal" # Instantiate a simulation sim = agxSDK.Simulation() # By default, the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that # too by creating an agx.PointGravityField for example). # AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen) if not GRAVITY: logger.info("Gravity off.") g = agx.Vec3(0, 0, 0) # remove gravity sim.setUniformGravity(g) # Get current delta-t (timestep) that is used in the simulation? dt = sim.getTimeStep() logger.debug("default dt = {}".format(dt)) # Change the timestep sim.setTimeStep(TIMESTEP) # Confirm timestep changed dt = sim.getTimeStep() logger.debug("new dt = {}".format(dt)) # Create a new empty Assembly scene = agxSDK.Assembly() scene.setName(assembly_name + "assembly") # Add start assembly to simulation sim.add(scene) # Create a ground plane for reference and obstacle if not goal: ground = create_body(name="ground", shape=agxCollide.Box(LENGTH, LENGTH, GROUND_WIDTH), position=agx.Vec3( 0, 0, -(GROUND_WIDTH + SIZE_GRIPPER / 2 + LENGTH)), motion_control=agx.RigidBody.STATIC) sim.add(ground) # Create two grippers gripper_left = create_body(name="gripper_left" + goal_string, shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER, SIZE_GRIPPER), position=agx.Vec3(-LENGTH / 2, 0, 0), motion_control=agx.RigidBody.DYNAMICS) scene.add(gripper_left) gripper_right = create_body(name="gripper_right" + goal_string, shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER, SIZE_GRIPPER), position=agx.Vec3(LENGTH / 2, 0, 0), motion_control=agx.RigidBody.DYNAMICS) scene.add(gripper_right) gripper_left_body = gripper_left.getRigidBody("gripper_left" + goal_string) gripper_right_body = gripper_right.getRigidBody("gripper_right" + goal_string) # Create material material_cylinder = agx.Material("cylinder_material") bulk_material_cylinder = material_cylinder.getBulkMaterial() bulk_material_cylinder.setPoissonsRatio(POISSON_RATIO) bulk_material_cylinder.setYoungsModulus(YOUNG_MODULUS) cylinder = create_body(name="obstacle" + goal_string, shape=agxCollide.Cylinder(CYLINDER_RADIUS, CYLINDER_LENGTH), position=agx.Vec3(0, 0, -2 * CYLINDER_RADIUS), motion_control=agx.RigidBody.STATIC, material=material_cylinder) scene.add(cylinder) # Create cable cable = agxCable.Cable(RADIUS, RESOLUTION) # Create Frames for each gripper: # Cables are attached passing through the attachment point along the Z axis of the body's coordinate frame. # The translation specified in the transformation is relative to the body and not the world left_transform = agx.AffineMatrix4x4() left_transform.setTranslate(SIZE_GRIPPER + RADIUS, 0, 0) left_transform.setRotate( agx.Vec3.Z_AXIS(), agx.Vec3.Y_AXIS()) # Rotation matrix which switches Z with Y frame_left = agx.Frame(left_transform) right_transform = agx.AffineMatrix4x4() right_transform.setTranslate(-SIZE_GRIPPER - RADIUS, 0, 0) right_transform.setRotate( agx.Vec3.Z_AXIS(), -agx.Vec3.Y_AXIS()) # Rotation matrix which switches Z with -Y frame_right = agx.Frame(right_transform) cable.add( agxCable.FreeNode(agx.Vec3(-LENGTH / 2 + SIZE_GRIPPER + RADIUS, 0, 0))) # Fix cable to gripper_left cable.add( agxCable.FreeNode(agx.Vec3(LENGTH / 2 - SIZE_GRIPPER - RADIUS, 0, 0))) # Fix cable to gripper_right # Set cable name and properties cable.setName("DLO" + goal_string) properties = cable.getCableProperties() properties.setYoungsModulus(YOUNG_MODULUS, agxCable.BEND) properties.setYoungsModulus(YOUNG_MODULUS, agxCable.TWIST) properties.setYoungsModulus(YOUNG_MODULUS, agxCable.STRETCH) material_wire = cable.getMaterial() wire_material = material_wire.getBulkMaterial() wire_material.setPoissonsRatio(POISSON_RATIO) wire_material.setYoungsModulus(YOUNG_MODULUS) cable.setMaterial(material_wire) # Add cable plasticity plasticity = agxCable.CablePlasticity() plasticity.setYieldPoint( YIELD_POINT, agxCable.BEND) # set torque required for permanent deformation plasticity.setYieldPoint( YIELD_POINT, agxCable.STRETCH) # set torque required for permanent deformation cable.addComponent(plasticity) # NOTE: Stretch direction is always elastic # Tell MaterialManager to create and return a contact material which will be used # when two geometries both with this material is in contact contact_material = sim.getMaterialManager().getOrCreateContactMaterial( material_cylinder, material_wire) contact_material.setYoungsModulus(CONTACT_YOUNG_MODULUS) # Create a Friction model, which we tell the solver to solve ITERATIVELY (faster) fm = agx.IterativeProjectedConeFriction() fm.setSolveType(agx.FrictionModel.DIRECT) contact_material.setFrictionModel(fm) # Try to initialize cable report = cable.tryInitialize() if report.successful(): logger.debug("Successful cable initialization.") else: logger.error(report.getActualError()) # Add cable to simulation sim.add(cable) # Add segment names and get first and last segment segment_count = 0 iterator = cable.begin() segment_left = iterator.getRigidBody() segment_left.setName('dlo_' + str(segment_count + 1) + goal_string) segment_right = None while not iterator.isEnd(): segment_count += 1 segment_right = iterator.getRigidBody() segment_right.setName('dlo_' + str(segment_count + 1) + goal_string) iterator.inc() # Add hinge constraints hinge_joint_left = agx.Hinge( sim.getRigidBody("gripper_left" + goal_string), frame_left, segment_left) hinge_joint_left.setName('hinge_joint_left' + goal_string) motor_left = hinge_joint_left.getMotor1D() motor_left.setEnable(False) motor_left_param = motor_left.getRegularizationParameters() motor_left_param.setCompliance(1e12) motor_left.setLockedAtZeroSpeed(False) lock_left = hinge_joint_left.getLock1D() lock_left.setEnable(False) range_left = hinge_joint_left.getRange1D() range_left.setEnable(True) range_left.setRange(agx.RangeReal(-math.pi / 2, math.pi / 2)) sim.add(hinge_joint_left) hinge_joint_right = agx.Hinge( sim.getRigidBody("gripper_right" + goal_string), frame_right, segment_right) hinge_joint_right.setName('hinge_joint_right' + goal_string) motor_right = hinge_joint_right.getMotor1D() motor_right.setEnable(False) motor_right_param = motor_right.getRegularizationParameters() motor_right_param.setCompliance(1e12) motor_right.setLockedAtZeroSpeed(False) lock_right = hinge_joint_right.getLock1D() lock_right.setEnable(False) range_right = hinge_joint_right.getRange1D() range_right.setEnable(True) range_right.setRange(agx.RangeReal(-math.pi / 2, math.pi / 2)) sim.add(hinge_joint_right) # Create bases for gripper motors prismatic_base_left = create_locked_prismatic_base( "gripper_left" + goal_string, gripper_left_body, compliance=0, motor_ranges=[(-FORCE_RANGE, FORCE_RANGE), (-FORCE_RANGE, FORCE_RANGE), (-FORCE_RANGE, FORCE_RANGE)], position_ranges=[(-LENGTH / 2 + CYLINDER_RADIUS, LENGTH / 2 - CYLINDER_RADIUS), (-CYLINDER_LENGTH / 3, CYLINDER_LENGTH / 3), (-(GROUND_WIDTH + SIZE_GRIPPER / 2 + LENGTH), 0)], lock_status=[False, False, False]) sim.add(prismatic_base_left) prismatic_base_right = create_locked_prismatic_base( "gripper_right" + goal_string, gripper_right_body, compliance=0, motor_ranges=[(-FORCE_RANGE, FORCE_RANGE), (-FORCE_RANGE, FORCE_RANGE), (-FORCE_RANGE, FORCE_RANGE)], position_ranges=[(-LENGTH / 2 + CYLINDER_RADIUS, LENGTH / 2 - CYLINDER_RADIUS), (-CYLINDER_LENGTH / 3, CYLINDER_LENGTH / 3), (-(GROUND_WIDTH + SIZE_GRIPPER / 2 + LENGTH), 0)], lock_status=[False, False, False]) sim.add(prismatic_base_right) return sim
def build_simulation(goal=False): """Builds simulations for both start and goal configurations :param bool goal: toggles between simulation definition of start and goal configurations :return agxSDK.Simulation: simulation object """ assembly_name = "start_" goal_string = "" if goal: assembly_name = "goal_" goal_string = "_goal" # Instantiate a simulation sim = agxSDK.Simulation() # By default, the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that # too by creating an agx.PointGravityField for example). # AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen) if not GRAVITY: logger.info("Gravity off.") g = agx.Vec3(0, 0, 0) # remove gravity sim.setUniformGravity(g) # Get current delta-t (timestep) that is used in the simulation? dt = sim.getTimeStep() logger.debug("default dt = {}".format(dt)) # Change the timestep sim.setTimeStep(TIMESTEP) # Confirm timestep changed dt = sim.getTimeStep() logger.debug("new dt = {}".format(dt)) # Create a new empty Assembly scene = agxSDK.Assembly() scene.setName(assembly_name + "assembly") # Add start assembly to simulation sim.add(scene) # Create a ground plane for reference if not goal: ground = create_body(name="ground", shape=agxCollide.Box(LENGTH, LENGTH, GROUND_WIDTH), position=agx.Vec3(LENGTH / 2, 0, -(GROUND_WIDTH + SIZE_GRIPPER / 2 + LENGTH)), motion_control=agx.RigidBody.STATIC) scene.add(ground) # Create cable cable = agxCable.Cable(RADIUS, RESOLUTION) cable.setName("DLO" + goal_string) gripper_left = create_body(name="gripper_left" + goal_string, shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER, SIZE_GRIPPER), position=agx.Vec3(0, 0, 0), motion_control=agx.RigidBody.DYNAMICS) scene.add(gripper_left) gripper_right = create_body(name="gripper_right" + goal_string, shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER, SIZE_GRIPPER), position=agx.Vec3(LENGTH, 0, 0), motion_control=agx.RigidBody.DYNAMICS) scene.add(gripper_right) # Disable collisions for grippers gripper_left_body = scene.getRigidBody("gripper_left" + goal_string) gripper_left_body.getGeometry("gripper_left" + goal_string).setEnableCollisions(False) gripper_right_body = scene.getRigidBody("gripper_right" + goal_string) gripper_right_body.getGeometry("gripper_right" + goal_string).setEnableCollisions(False) logger.info("Mass of grippers: {}".format(scene.getRigidBody("gripper_right" + goal_string).calculateMass())) # Create Frames for each gripper: # Cables are attached passing through the attachment point along the Z axis of the body's coordinate frame. # The translation specified in the transformation is relative to the body and not the world left_transform = agx.AffineMatrix4x4() left_transform.setTranslate(SIZE_GRIPPER + RADIUS, 0, 0) left_transform.setRotate(agx.Vec3.Z_AXIS(), agx.Vec3.Y_AXIS()) # Rotation matrix which switches Z with Y frame_left = agx.Frame(left_transform) right_transform = agx.AffineMatrix4x4() right_transform.setTranslate(- SIZE_GRIPPER - RADIUS, 0, 0) right_transform.setRotate(agx.Vec3.Z_AXIS(), -agx.Vec3.Y_AXIS()) # Rotation matrix which switches Z with -Y frame_right = agx.Frame(right_transform) cable.add(agxCable.FreeNode(agx.Vec3(SIZE_GRIPPER + RADIUS, 0, 0))) # Fix cable to gripper_left cable.add(agxCable.FreeNode(agx.Vec3(LENGTH - SIZE_GRIPPER - RADIUS, 0, 0))) # Fix cable to gripper_right # Try to initialize cable report = cable.tryInitialize() if report.successful(): logger.debug("Successful cable initialization.") else: logger.error(report.getActualError()) actual_length = report.getLength() logger.info("Actual length: " + str(actual_length)) # Add cable plasticity plasticity = agxCable.CablePlasticity() plasticity.setYieldPoint(YIELD_POINT, agxCable.BEND) # set torque required for permanent deformation cable.addComponent(plasticity) # NOTE: Stretch direction is always elastic # Define material material = agx.Material("Aluminum") bulk_material = material.getBulkMaterial() bulk_material.setPoissonsRatio(POISSON_RATIO) bulk_material.setYoungsModulus(YOUNG_MODULUS) cable.setMaterial(material) # Add cable to simulation scene.add(cable) # Add segment names and get first and last segment count = 1 iterator = cable.begin() segment_left = iterator.getRigidBody() segment_left.setName('dlo_' + str(count) + goal_string) while not iterator.isEnd(): count += 1 segment_right = iterator.getRigidBody() segment_right.setName('dlo_' + str(count) + goal_string) iterator.inc() # Add hinge constraints hinge_joint_left = agx.Hinge(scene.getRigidBody("gripper_left" + goal_string), frame_left, segment_left) hinge_joint_left.setName('hinge_joint_left' + goal_string) motor_left = hinge_joint_left.getMotor1D() motor_left.setEnable(True) motor_left_param = motor_left.getRegularizationParameters() motor_left_param.setCompliance(1e12) motor_left.setLockedAtZeroSpeed(False) lock_left = hinge_joint_left.getLock1D() lock_left.setEnable(False) # Set range of hinge joint # range_left = hinge_joint_left.getRange1D() # range_left.setEnable(True) # range_left.setRange(agx.RangeReal(-math.pi / 2, math.pi / 2)) scene.add(hinge_joint_left) hinge_joint_right = agx.Hinge(scene.getRigidBody("gripper_right" + goal_string), frame_right, segment_right) hinge_joint_right.setName('hinge_joint_right' + goal_string) motor_right = hinge_joint_right.getMotor1D() motor_right.setEnable(True) motor_right_param = motor_right.getRegularizationParameters() motor_right_param.setCompliance(1e12) motor_right.setLockedAtZeroSpeed(False) lock_right = hinge_joint_right.getLock1D() lock_right.setEnable(False) # Set range of hinge joint # range_right = hinge_joint_right.getRange1D() # range_right.setEnable(True) # range_right.setRange(agx.RangeReal(-math.pi / 2, math.pi / 2)) scene.add(hinge_joint_right) # Create base for gripper motors prismatic_base_right = create_locked_prismatic_base("gripper_right" + goal_string, gripper_right_body, compliance=0, motor_ranges=[(-FORCE_RANGE, FORCE_RANGE), (-FORCE_RANGE, FORCE_RANGE), (-FORCE_RANGE, FORCE_RANGE)], position_ranges=[ (-LENGTH + 2 * (2 * RADIUS + SIZE_GRIPPER), 0), (-LENGTH, LENGTH), (-LENGTH, LENGTH)], compute_forces=True, lock_status=[False, False, False]) scene.add(prismatic_base_right) prismatic_base_left = create_locked_prismatic_base("gripper_left" + goal_string, gripper_left_body, compliance=0, lock_status=[True, True, True]) scene.add(prismatic_base_left) return sim