def PreSetup(args, SetupFunction):
if agxPython.getContext():
return
init = agx.AutoInit()
## Create an application with graphics etc.
app = agxOSG.ExampleApplication()
dec = app.getSceneDecorator()
dec.setBackgroundColor(agx.Vec3(1,1,1))
dec.setLogoFile('textures/TF-loader.png')
dec.setLogoLocation(agxOSG.SceneDecorator.FREE)
width = 0.25
dec.setLogoPosition(0.5-width/2, 0.3)
dec.setMaximumLogoDimension(width, 1.0)
## Create a command line parser. sys.executable will point to python executable
## in this case, because getArgumentName(0) needs to match the C argv[0] which
## is the name of the program running
argParser = agxIO.ArgumentParser([sys.executable] + args)
app.addScene(argParser.getArgumentName(1), "buildScene", ord('1'), True)
## Call the init method of ExampleApplication
## It will setup the viewer, windows etc.
if app.init(argParser):
app.run()
else:
print("An error occurred while initializing ExampleApplication.")
def main(args):
# Build simulation object
sim = build_simulation()
# Save simulation to file
success = save_simulation(sim, FILE_NAME)
if success:
logger.debug("Simulation saved!")
else:
logger.debug("Simulation not saved!")
# Add app
app = add_rendering(sim)
app.init(
agxIO.ArgumentParser([sys.executable, '--window', '400', '600'] +
args))
app.setTimeStep(TIMESTEP)
app.setCameraHome(EYE, CENTER, UP)
app.initSimulation(sim, True)
cylinder_pos_x = np.random.uniform(-0.1, 0.1)
cylinder_pos_y = np.random.uniform(0.05, 0.05)
cylinder = sim.getRigidBody("hollow_cylinder")
cylinder.setPosition(agx.Vec3(cylinder_pos_x, cylinder_pos_y, 0.0))
segment_pos_old = compute_segments_pos(sim)
reward_type = "dense"
for _ in range(10000):
sim.stepForward()
app.executeOneStepWithGraphics()
# Get segments positions
segment_pos = compute_segments_pos(sim)
# Compute reward
if reward_type == "sparse":
reward, goal_reached = compute_dense_reward_and_check_goal(
sim, segment_pos, segment_pos_old)
else:
goal_reached = is_goal_reached(sim)
reward = float(goal_reached)
segment_pos_old = segment_pos
if reward != 0:
print("reward: ", reward)
if goal_reached:
print("Success!")
break
def main(args):
# 1) Build start simulation object
sim = build_simulation()
# Save start simulation to file
success = save_simulation(sim, FILE_NAME)
if not success:
logger.debug("Simulation not saved!")
# 2) Build goal simulation object
goal_sim = build_simulation(goal=True)
# Render simulation
app = add_rendering(goal_sim)
app.init(agxIO.ArgumentParser([sys.executable] + args))
app.setCameraHome(EYE, CENTER, UP) # should only be added after app.init
app.initSimulation(goal_sim, True) # This changes timestep and Gravity!
goal_sim.setTimeStep(TIMESTEP)
if not GRAVITY:
logger.info("Gravity off.")
g = agx.Vec3(0, 0, 0) # remove gravity
goal_sim.setUniformGravity(g)
# 3) Sample fixed goal
sample_fixed_goal(goal_sim, app)
# Set goal objects to static
rbs = goal_sim.getRigidBodies()
for rb in rbs:
name = rb.getName()
if "_goal" in name:
rb.setMotionControl(agx.RigidBody.STATIC)
# Save fixed goal simulation to file
success = save_simulation(goal_sim, FILE_NAME + "_goal")
if not success:
logger.debug("Fixed goal simulation not saved!")
# 4) Build random goal simulation object (without DLO)
random_goal_sim = build_simulation(goal=True, rope=False)
success = save_simulation(random_goal_sim, FILE_NAME + "_goal_random", aagx=True)
if not success:
logger.debug("Goal simulation not saved!")
file_directory = os.path.dirname(os.path.abspath(__file__))
package_directory = os.path.split(file_directory)[0]
random_goal_file = os.path.join(package_directory, 'envs/assets', FILE_NAME + "_goal_random.agx")
add_goal_assembly_from_file(sim, random_goal_file)
# Test random goal generation
sample_random_goal(sim, render=True)
def main(args):
"""
Script main. Called when run with the native Python interpreter
instead of with agxViewer.
"""
app = agxOSG.ExampleApplication()
arg_parser = agxIO.ArgumentParser([sys.executable] + args)
app.addScene(arg_parser.getArgumentName(1), "build_scene", ord('1'), False)
if app.init(arg_parser):
app.run()
rclpy.shutdown() # Shut down ROS2
else:
print("An error occurred while initializing agx application.")
def main(args):
# Build simulation object
sim = build_simulation()
# Save simulation to file
success = save_simulation(sim, FILE_NAME)
if success:
logger.debug("Simulation saved!")
else:
logger.debug("Simulation not saved!")
# Add app
app = add_rendering(sim)
app.init(
agxIO.ArgumentParser([sys.executable, '--window', '600', '600'] +
args))
app.setTimeStep(TIMESTEP)
app.setCameraHome(EYE, CENTER, UP)
app.initSimulation(sim, True)
segment_pos_old = compute_segments_pos(sim)
reward_type = "dense"
# Set random obstacle position
# center_pos = np.random.uniform([-MAX_X, -MAX_Y], [MAX_X, MAX_Y])
center_pos = np.array([-MAX_X, -MAX_Y])
set_center_obstacle(sim, center_pos)
for _ in range(10000):
sim.stepForward()
app.executeOneStepWithGraphics()
# Get segments positions
segment_pos = compute_segments_pos(sim)
# Compute reward
if reward_type == "dense":
reward, goal_reached = compute_dense_reward_and_check_goal(
center_pos, segment_pos, segment_pos_old)
else:
goal_reached = is_goal_reached(center_pos, segment_pos)
reward = float(goal_reached)
segment_pos_old = segment_pos
if reward != 0:
print("reward: ", reward)
if goal_reached:
print("Success!")
break
def main(args):
# Build simulation object
sim = build_simulation()
# Save simulation to file
success = save_simulation(sim, FILE_NAME)
if success:
logger.debug("Simulation saved!")
else:
logger.debug("Simulation not saved!")
# Add app
app = add_rendering(sim)
app.init(agxIO.ArgumentParser([sys.executable, '--window', '600', '600'] + args))
app.setTimeStep(TIMESTEP)
app.setCameraHome(EYE, CENTER, UP)
app.initSimulation(sim, True)
# Randomize goal position
goal_pos_new = np.random.uniform([-0.1, -0.1], [0.1, -0.025])
sim.getRigidBody("obstacle_goal").setPosition(agx.Vec3(goal_pos_new[0], goal_pos_new[1] ,0.005))
reward_type = "sparse"
segment_pos_old = compute_segments_pos(sim)
for _ in range(10000):
sim.stepForward()
app.executeOneStepWithGraphics()
# Compute reward
segment_pos = compute_segments_pos(sim)
# Compute reward
if reward_type == "dense":
reward, goal_reached = compute_dense_reward_and_check_goal(sim, segment_pos, segment_pos_old)
else:
goal_reached = is_goal_reached(sim, segment_pos)
reward = float(goal_reached)
segment_pos_old = segment_pos
if reward !=0:
print("reward: ", reward)
if goal_reached:
print("Success!")
break
def main(args):
## Create an application with graphics etc.
app = agxOSG.ExampleApplication()
## Create a command line parser. sys.executable will point to python executable
## in this case, because getArgumentName(0) needs to match the C argv[0] which
## is the name of the program running
argParser = agxIO.ArgumentParser([sys.executable] + args)
app.addScene(argParser.getArgumentName(1), "buildScene", ord('1'), True)
## Call the init method of ExampleApplication
## It will setup the viewer, windows etc.
if app.init(argParser):
app.run()
else:
print("An error occurred while initializing ExampleApplication.")
def _applicationMain(**kwargs):
## Create an application with graphics etc.
app = agxOSG.ExampleApplication()
app.setAutoStepping(kwargs.get('autoStepping', False))
## Create a command line parser. sys.executable will point to python executable
## in this case, because getArgumentName(0) needs to match the C argv[0] which
## is the name of the program running
argParser = agxIO.ArgumentParser([sys.executable] + sys.argv)
global _unittestAvailable
if (_unittestAvailable):
agxUnit.initUnittestFrameWork(argParser)
if 'scenes' in kwargs:
for sceneData in kwargs['scenes']:
scene = sceneData[0].__name__ if callable(
sceneData[0]) else sceneData[0]
key = ord(sceneData[1]) if isinstance(sceneData[1],
str) else sceneData[1]
asTest = sceneData[2] if len(
sceneData
) > 2 else True # 'is part of unittest' True by default
stopAfter = sceneData[3] if len(sceneData) > 3 else 0.0
app.addScene(argParser.getArgumentName(1), scene, key, asTest,
stopAfter)
## Call the init method of ExampleApplication
## It will setup the viewer, windows etc.
if app.init(argParser):
# Avoid 'start paused' during tests.
if unittestEnabled() or argParser.find('--agxOnly') >= 0:
app.setAutoStepping(True)
if 'onInitialized' in kwargs and callable(kwargs['onInitialized']):
kwargs['onInitialized'](app)
app.run()
if 'onShutdown' in kwargs and callable(kwargs['onShutdown']):
kwargs['onShutdown'](app)
else:
print("An error occurred while initializing ExampleApplication.")
def createSimulation(self, no_graphics=True):
ap = argparse.ArgumentParser()
# the --noApp will run this without a graphics window
ap.add_argument('--noApp', action='store_true')
args1, args2 = ap.parse_known_args()
args1 = vars(args1)
if no_graphics:
app = None
else:
# Creates an Example Application
app = agxOSG.ExampleApplication()
app.init(agxIO.ArgumentParser([sys.executable] + args2))
# Create a simulation
sim = agxSDK.Simulation()
return sim, app
def _init_app(self):
"""Initialize OSG Example Application. Needed for rendering graphics.
:param bool add_background_rgb: flag to determine if type of background rendering is RGB.
:param bool add_background_depth: flag to determine if type of background rendering is depth.
"""
logger.info("init app")
self.app.init(agxIO.ArgumentParser([sys.executable] + self.args))
self.app.setCameraHome(self.camera_pose['eye'],
self.camera_pose['center'],
self.camera_pose['up']) # only after app.init
self.app.initSimulation(self.sim, True) # initialize graphics
if self.observation_type == "rgb" or self.observation_type == "rgb_and_depth":
self._add_background_rendering()
if self.observation_type == "depth" or self.observation_type == "rgb_and_depth":
self._add_background_rendering(depth=True)
self.sim.setUniformGravity(self.gravity)
self.sim.setTimeStep(self.time_step)
logger.debug("Timestep after initSimulation is: {}".format(self.sim.getTimeStep()))
logger.debug("Gravity after initSimulation is: {}".format(self.sim.getUniformGravity()))
def main(args):
# Build simulation object
sim = build_simulation()
# Save simulation to file
success = save_simulation(sim, FILE_NAME)
if success:
logger.debug("Simulation saved!")
else:
logger.debug("Simulation not saved!")
# Render simulation
app = add_rendering(sim)
app.init(agxIO.ArgumentParser([sys.executable] + args))
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 = 30
n_steps = int(n_seconds / (TIMESTEP * N_SUBSTEPS))
for k in range(n_steps):
app.executeOneStepWithGraphics()
t = sim.getTimeStamp()
t_0 = t
while t < t_0 + TIMESTEP * N_SUBSTEPS:
sim.stepForward()
t = sim.getTimeStamp()
# Save goal simulation to file
success = save_simulation(sim, FILE_NAME + "_goal")
if success:
logger.debug("Goal simulation saved!")
else:
logger.debug("Goal simulation not saved!")
if agxPython.getContext() is None:
import os
if os.name == "posix":
agx_dir = os.environ['AGX_DIR']
agxIO.Environment_instance().getFilePath(
agxIO.Environment.RESOURCE_PATH).addFilePath(agx_dir)
agxIO.Environment_instance().getFilePath(
agxIO.Environment.RESOURCE_PATH).addFilePath(agx_dir + "/data")
init = agx.AutoInit()
import sys
argParser = agxIO.ArgumentParser([sys.executable] + sys.argv)
example_app = agxOSG.ExampleApplication()
example_app.addScene(argParser.getArgumentName(1), "build_scene", ord('1'),
True)
if example_app.init(argParser):
example_app.run()
else:
print("An error occurred while initializing ExampleApplication.")
# ---------------- SETTERS OR GETTERS ------------------
def set_aft_motor_angle(angle):
aftMotorAngle = math.degrees(angle)
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()