Esempio n. 1
0
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
Esempio n. 2
0
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
Esempio n. 3
0
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
Esempio n. 4
0
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
Esempio n. 5
0
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
Esempio n. 6
0
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
Esempio n. 7
0
def sample_random_goal(sim, render=False):
    """Goal Randomization: for the PushRope environment it is too difficult to generate proper trajectories that lead to
     varied shapes. For this reason, a new rope is added to the scene every time, and routed through random points
    :param sim: AGX Dynamics simulation object
    :param bool render: toggle rendering for debugging purposes only
    """
    # get goal assembly
    goal_scene = sim.getAssembly("goal_assembly")

    # Create rope
    rope = agxCable.Cable(RADIUS, RESOLUTION)
    rope_z = RADIUS

    # Create random positions for first node
    new_node_x = random.uniform((-0.9 * LENGTH + RADIUS), (0.9 * LENGTH - RADIUS))
    new_node_y = random.uniform((-0.9 * LENGTH + RADIUS), (0.9 * LENGTH - RADIUS))

    # compute angle pointing towards center
    # rope_angle = math.atan2(-new_node_y, -new_node_x)

    # Uniformly distributed initial angle
    rope_angle = random.uniform(-math.pi, math.pi)

    rope.add(agxCable.FreeNode(new_node_x, new_node_y, rope_z))

    # compute length of routing sections
    section_length = LENGTH / (NODE_AMOUNT-1)

    for i in range(NODE_AMOUNT-1):
        # modify previous angle and calculate new node coordinates
        rope_angle += random.gauss(0, math.pi / 4)

        prev_node_x = new_node_x
        prev_node_y = new_node_y

        new_node_x = prev_node_x + math.cos(rope_angle) * section_length
        new_node_y = prev_node_y + math.sin(rope_angle) * section_length

        # if node ends up too close to the borders, reset angle to point towards center
        while abs(new_node_x) / LENGTH > 0.9 or abs(new_node_y) / LENGTH > 0.9:
            # intentionally using the new coordinates for additional randomization
            rope_angle = math.atan2(-new_node_y, -new_node_x)
            rope_angle += random.gauss(0, math.pi / 4)

            new_node_x = prev_node_x + math.cos(rope_angle) * section_length
            new_node_y = prev_node_y + math.sin(rope_angle) * section_length

        rope.add(agxCable.FreeNode(new_node_x, new_node_y, rope_z))

    # Set rope name and properties
    rope.setName("DLO_goal")
    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():
        logger.info("Successful rope initialization.")
    else:
        print(report.getActualError())

    # Add rope to simulation
    goal_scene.add(rope)

    start_scene = sim.getAssembly("start_assembly")
    agxUtil.setEnableCollisions(goal_scene, start_scene, False)  # need to disable collisions again after adding rope

    # 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)

    # simulate for a short while without graphics to smoothen out kinks at the routing nodes
    for _ in range(1000):
        sim.stepForward()

    # reset timestamp, after simulation
    sim.setTimeStamp(0)

    rbs = rope.getRigidBodies()
    for i, rb in enumerate(rbs):
        rbs[i].setName('dlo_' + str(i + 1) + '_goal')
        rb.setMotionControl(agx.RigidBody.STATIC)

    if render:
        # Add keyboard listener
        motor_x = sim.getConstraint1DOF("pusher_joint_base_x").getMotor1D()
        motor_y = sim.getConstraint1DOF("pusher_joint_base_y").getMotor1D()
        motor_z = sim.getConstraint1DOF("pusher_joint_base_z").getMotor1D()
        key_motor_map = {agxSDK.GuiEventListener.KEY_Up: (motor_y, 0.05),
                         agxSDK.GuiEventListener.KEY_Down: (motor_y, -0.05),
                         agxSDK.GuiEventListener.KEY_Right: (motor_x, 0.05),
                         agxSDK.GuiEventListener.KEY_Left: (motor_x, -0.05),
                         65365: (motor_z, 0.05),
                         65366: (motor_z, -0.05)}
        sim.add(KeyboardMotorHandler(key_motor_map))

        # Render simulation
        app = add_rendering(sim)
        app.init(agxIO.ArgumentParser([sys.executable]))  # no args being passed to agxViewer!
        app.setCameraHome(EYE, CENTER, UP)  # should only be added after app.init
        app.initSimulation(sim, True)  # This changes timestep and Gravity!
        sim.setTimeStep(TIMESTEP)
        if not GRAVITY:
            logger.info("Gravity off.")
            g = agx.Vec3(0, 0, 0)  # remove gravity
            sim.setUniformGravity(g)

        n_seconds = 10
        t = sim.getTimeStamp()
        while t < n_seconds:
            app.executeOneStepWithGraphics()

            t = sim.getTimeStamp()
            t_0 = t
            while t < t_0 + TIMESTEP * N_SUBSTEPS:
                sim.stepForward()
                t = sim.getTimeStamp()