def create_sphere(position: agx.Vec3, scale): shape = agxCollide.Sphere(0.5 * scale) geometry = agxCollide.Geometry(shape) body = agx.RigidBody(geometry) body.setPosition(position) oneLegRobotApp.create_visual(body) return body
def create_sphere(position: agx.Vec3): shape = agxCollide.Sphere(0.5) geometry = agxCollide.Geometry(shape) body = agx.RigidBody(geometry) body.setPosition(position) demoutils.create_visual(body) return body
def add_stud_sensor(self, props, x, y): """ Adds sensor at stud for peg in hole simulation """ stud_sensor = agxCollide.Geometry(agxCollide.Sphere(props.stud_radius)) stud_sensor.addGroup("stud") stud_sensor.setSensor(True) self.brick_rb.add(stud_sensor) stud_sensor.setLocalPosition(x, y, props.height + props.studcyl_height - props.stud_radius)
def addball(sim, root, rad, pos, Fixed=True): if type(pos) == type([]): pos = agx.Vec3(pos[0], pos[1], pos[2]) ball = agx.RigidBody( agxCollide.Geometry( agxCollide.Sphere(rad))) ball.setPosition(pos) if(Fixed): ball.setMotionControl(1) sim.add(ball) agxOSG.setDiffuseColor(agxOSG.createVisual(ball, root), agxRender.Color.Red()) return ball
def build_scene1(): # Create a geometry with a plane shape as our 'floor' floor_geometry = agxCollide.Geometry(agxCollide.Box(agx.Vec3(10, 10, 0.1))) demoutils.create_visual(floor_geometry) demoutils.sim().add(floor_geometry) # Add the geometry to the simulation rb1 = agx.RigidBody() # Create a rigid body rb1.add(agxCollide.Geometry(agxCollide.Sphere( 0.5))) # Add a geometry with a sphere-shape of radius 0.5 rb1.setPosition(0, 0, 5.0) # Position the sphere somewhere above our plane demoutils.create_visual(rb1) demoutils.sim().add( rb1) # Add the body to the simulation. The geometry will also be added
def add_sphereShape(MiroSystem, radius, pos, texture='test.jpg', density=1000, 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 box body_geo = agxCollide.Geometry(agxCollide.Sphere(radius)) body_geo.setName("body") body_geo.setEnableCollisions(Collide) body_ball = agx.RigidBody(body_geo) if Fixed: body_ball.setMotionControl(1) body_ball.setPosition(agxPos) rotateBody(body_ball, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees) # Collision shape # if(Collide): # Visualization shape body_shape = agxOSG.createVisual(body_ball, 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_ball) return body_ball
def add_hole_sensor(self, props, x, y, listener_pool): """ Create a sensor geometry spere to represent the hole. When a stud sensor overlaps with this sphere a prismatic joint is created to simulate the holes friction """ hole_sensor = agxCollide.Geometry(agxCollide.Sphere(props.stud_radius)) hole_sensor.setSensor(True) self.brick_rb.add(hole_sensor) hole_sensor.setLocalPosition(x, y, props.stud_radius) self.hole_sensors.append(hole_sensor) contact_listener = listener_pool.get_listener() contact_listener.hole_depth = props.studcyl_height contact_listener.setFilter(agxSDK.GeometryFilter(hole_sensor)) contact_listener.setEnable(True) self.listeners.append(contact_listener)
def build_scene2(): # Create a geometry with a plane shape as our 'floor' floor_geometry = agxCollide.Geometry(agxCollide.Box(agx.Vec3(10, 10, 0.1))) demoutils.create_visual(floor_geometry, diffuse_color=Color.Green()) demoutils.sim().add(floor_geometry) # Add the geometry to the simulation for x in range(-5, 5): for y in range(-5, 5): for z in range(1, 8): rb = agx.RigidBody() # Create a rigid body rb.add(agxCollide.Geometry(agxCollide.Sphere( 0.2))) # Add a geometry with a sphere-shape of radius 0.2 rb.setPosition( x + random(), y + random(), z) # Position the sphere somewhere above our plane demoutils.create_visual(rb) demoutils.sim().add( rb ) # Add the body to the simulation. The geometry will also be added
def build_scene(): # application entry point. Do not change method signature snakeapp.register_additional_scenes('build_scene_2') snake = Snake(NUM_SNAKE_MODULES, pitch_only=False)#, with_camera=True) # type: snake_module.Snake snake.setPosition(agx.Vec3(0, 0, 0.1)) snakeapp.add(snake) plane = agxCollide.Geometry(agxCollide.Box(2, 2, 0.1), agx.AffineMatrix4x4.translate(0, 0, -0.1/2)) snakeapp.create_visual(plane, diffuse_color=agxRender.Color.Green()) snakeapp.add(plane) ball = agxCollide.Geometry(agxCollide.Sphere(0.035), agx.AffineMatrix4x4.translate(0, 0, 0)) snakeapp.create_visual(ball, diffuse_color=agxRender.Color.YellowGreen()) ball.setPosition(agx.Vec3(-0.5, 0, 0.1)) snakeapp.add(ball) snakeapp.add_event_listener(MyKeyEvent(ball)) snakeapp.init_camera(eye=agx.Vec3(-1, -1, 0.5)) snake_controller = SnakeBallControl(snake, ball) snake_controller.init_turning(math.pi / 9.0, math.pi * 2.0 / 3.0, 8.0, 0.0, math.pi * 0.0 / 180.0) snakeapp.add_event_listener(snake_controller)
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(): # 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
import agx import agxCollide init = agx.AutoInit() shape = agxCollide.Sphere(0.5) geometry = agxCollide.Geometry(shape) geometry.setEnableCollisions( False) # Geometry will not collide with other objects body = agx.RigidBody(geometry) body.setMotionControl(agx.RigidBody.DYNAMICS) # Default value
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