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_capsule(position: agx.Vec3): shape = agxCollide.Capsule(0.5, 1) geometry = agxCollide.Geometry(shape) body = agx.RigidBody(geometry) body.setPosition(position) demoutils.create_visual(body) body.setRotation(agx.EulerAngles(agx.PI_2, 0, 0)) return body
def create_capsule(position: agx.Vec3, scale): shape = agxCollide.Capsule(0.5 * scale, 1 * scale) geometry = agxCollide.Geometry(shape) body = agx.RigidBody(geometry) body.setPosition(position) oneLegRobotApp.create_visual(body) body.setRotation(agx.EulerAngles(agx.PI_2, 0, 0)) return body
def create_fenders(self, fender_material, r_fender, half_width, half_height, half_length): fender_color = agxRender.Color.Black() fender = agxCollide.Geometry(agxCollide.Capsule(0.2, half_width * 2)) fender.setPosition(half_length, 0, 0) self.m_body.add(fender) agxOSG.setDiffuseColor(agxOSG.createVisual(fender, demoutils.root()), fender_color) fender.setMaterial(fender_material)
def create_fenders(fender_material, half_width, half_length): fender = agxCollide.Geometry(agxCollide.Capsule(0.8, half_width * 2)) fender.setPosition(half_length, 0, 0.2) fender.setMaterial(fender_material) return fender
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 setup_geometries(self, root, material, visual): # pylint: disable=too-many-statements, too-many-locals """ Create bodies and constraints so that we can move the gripper left/right/front/back """ translate_bodies = [agx.RigidBody(), agx.RigidBody()] move_direction = [agx.Vec3(-1, 0, 0), agx.Vec3(0, -1, 0)] for i in range(0, 2): body = translate_bodies[i] body.setLocalPosition(0, 0, 0) body.getMassProperties().setMass(self.props.mass) self.gripper.add(body) if i == 0: frame1 = agx.Frame() frame1.setLocalTranslate(body.getPosition()) frame1.setLocalRotate( agx.Quat(agx.Vec3(0, 0, -1), move_direction[i])) prismatic = agx.Prismatic(body, frame1) else: frame1 = agx.Frame() frame2 = agx.Frame() assert agx.Constraint.calculateFramesFromBody(agx.Vec3(), \ move_direction[i], translate_bodies[0], frame1, body, frame2) prismatic = agx.Prismatic(translate_bodies[0], frame1, body, frame2) self.gripper.add(prismatic) prismatic.getMotor1D().setLockedAtZeroSpeed(True) prismatic.getMotor1D().setForceRange(agx.RangeReal(50)) # N prismatic.getMotor1D().setSpeed(0) prismatic.getMotor1D().setEnable(True) prismatic.setSolveType(agx.Constraint.DIRECT_AND_ITERATIVE) prismatic.setCompliance(1e-15) self.move_constraints.append(prismatic) # Create a Cylindrical constraint that can be used for lifting AND rotating lift_rotate_body = agx.RigidBody() lift_rotate_body.setLocalPosition(0, 0, 0) lift_rotate_body.getMassProperties().setMass(self.props.mass) self.gripper.add(lift_rotate_body) frame1 = agx.Frame() frame2 = agx.Frame() assert agx.Constraint.calculateFramesFromBody( \ agx.Vec3(), agx.Vec3(0, 0, -1), translate_bodies[1], frame1, lift_rotate_body, frame2) self.cylindrical = agx.CylindricalJoint(translate_bodies[1], frame1, lift_rotate_body, frame2) self.cylindrical.setSolveType(agx.Constraint.DIRECT_AND_ITERATIVE) self.cylindrical.setCompliance(1e-15) # Enable the motors and set some properties on the motors for d in [agx.Constraint2DOF.FIRST, agx.Constraint2DOF.SECOND]: self.cylindrical.getMotor1D(d).setEnable(True) self.cylindrical.getMotor1D(d).setLockedAtZeroSpeed(True) self.cylindrical.getMotor1D(d).setSpeed(0) if d == agx.Constraint2DOF.FIRST: self.cylindrical.getMotor1D(d).setForceRange( agx.RangeReal(15)) # N else: self.cylindrical.getMotor1D(d).setForceRange( agx.RangeReal(50)) # Nm self.gripper.add(self.cylindrical) # Create the actual fingers that is used for picking up screws. capsule1 = agxCollide.Geometry( agxCollide.Capsule(self.props.radius, self.props.height)) capsule1.setLocalRotation(agx.EulerAngles(math.radians(90), 0, 0)) capsule1.setMaterial(material) capsule2 = capsule1.clone() capsule1.setLocalPosition(0, -self.props.radius, 0) capsule2.setLocalPosition(0, self.props.radius, 0) self.finger1 = agx.RigidBody() self.finger1.add(capsule1) self.finger1.add(capsule2) self.finger2 = self.finger1.clone() fingers = [self.finger1, self.finger2] # Create the constraints that is used for open/close the picking device direction = [1, -1] self.grip_constraints = [] grip_range = 0.025 self.grip_offs = self.props.radius for i in range(0, 2): finger = fingers[i] finger.setLocalPosition( direction[i] * (self.grip_offs + self.posref.grip1), 0, 0) self.gripper.add(finger) finger.getMassProperties().setMass(self.props.mass) frame1 = agx.Frame() frame2 = agx.Frame() assert agx.Constraint.calculateFramesFromBody( \ agx.Vec3(), agx.Vec3(-1, 0, 0) * direction[i], lift_rotate_body, frame1, finger, frame2) prismatic = agx.Prismatic(lift_rotate_body, frame1, finger, frame2) prismatic.getMotor1D().setForceRange(agx.RangeReal(-20, 20)) prismatic.getMotor1D().setLockedAtZeroSpeed(True) prismatic.getMotor1D().setEnable(True) prismatic.getRange1D().setRange( agx.RangeReal(-self.posref.grip1, grip_range - self.posref.grip1)) prismatic.getRange1D().setEnable(True) prismatic.setSolveType(agx.Constraint.DIRECT_AND_ITERATIVE) prismatic.setCompliance(1e-15) self.gripper.add(prismatic) self.grip_constraints.append(prismatic) self.sim.add(self.gripper) if visual: agxOSG.createVisual(self.gripper, root)