def init(visual, timestep):
""" Initialize ROS2 and various other things common to all agx simulations """
try:
rclpy.init()
except Exception:
if not rclpy.ok():
print("Init error, not because of multiple init")
if visual:
app = agxPython.getContext().environment.getApplication()
sim = agxPython.getContext().environment.getSimulation()
root = agxPython.getContext().environment.getSceneRoot()
else:
app = []
sim = agxSDK.Simulation()
root = []
sim.setTimeStep(timestep)
if visual:
setup_camera(app)
sim.add(GuiListener(app, timestep))
sim.getRenderManager().setScaleFactor(0.03)
app.getSceneDecorator().setBackgroundColor(agxRender.Color.SkyBlue(), agxRender.Color.DodgerBlue())
agx.setNumThreads(2)
return sim, root
def __init__(self, **kwargs):
"""
Construct given optional arguments.
Arguments:
model_path: str - path to model.
driveline_settings: Struct - driveline settings.
tire_settings: Struct - tire settings.
keyboard_controls: Struct - keyboard controls (optional)
gamepad_controls: Struct - gamepad controls (optional)
"""
super().__init__()
temp_simulation = agxSDK.Simulation()
data = SimulationFile(simulation=temp_simulation, root=root())
if not data.load(filename=kwargs['model_path'], parent=self):
raise FileNotFoundError(
'Unable to load wheel loader model: {}'.format(
kwargs['model_path']))
states = {}
for rb in self.getRigidBodies():
states[rb.getUuid()] = rb.getEnable()
disabled_collisions = DisabledCollisionsStateHandler(temp_simulation)
for disabled in disabled_collisions:
disabled1 = disabled[0]
disabled2 = disabled[1] if len(disabled) > 1 else disabled[0]
if isinstance(disabled1, agxCollide.Geometry):
continue
global_simulation().getSpace().setEnablePair(
disabled1, disabled2, False)
self._initialize(data, **kwargs)
del disabled_collisions
del data
del temp_simulation
for rb in self.getRigidBodies():
if not rb.getUuid() in states:
continue
rb.setEnable(states[rb.getUuid()])
from . import BucketTiltController
self.bucket_tilt_controller = kwargs.get('bucket_tilt_controller',
BucketTiltController(self))
assert self.tire( Tire.Location.FRONT_LEFT ).material == self.tire( Tire.Location.FRONT_RIGHT ).material,\
'Front left and right tire materials expected to be the same.'
assert self.tire( Tire.Location.REAR_LEFT ).material == self.tire( Tire.Location.REAR_RIGHT ).material,\
'Rear left and right tire materials expected to be the same.'
assert self.tire( Tire.Location.FRONT_LEFT ).material != self.tire( Tire.Location.REAR_LEFT ).material,\
'Front and rear tire materials not expected to be the same.'
def __init__(self, scene_path, n_substeps, n_actions, observation_type, image_size, camera_pose, no_graphics, args):
"""Initializes a AgxEnv object
:param scene_path: path to binary file containing serialized simulation defined in sim/ folder
:param n_substeps: number os simulation steps per call to step()
:param n_actions: number of actions (DoF)
:param camera_pose: dictionary containing EYE, CENTER, UP information for rendering
:param args: arguments for agxViewer
"""
self.scene_path = scene_path
self.n_substeps = n_substeps
self.render_to_image = []
self.image_size = image_size
self.no_graphics = no_graphics
# Initialize AGX simulation
self.gravity = None
self.time_step = None
self.init = agx.AutoInit()
self.sim = agxSDK.Simulation()
self._build_simulation()
# Initialize OSG ExampleApplication
self.app = agxOSG.ExampleApplication(self.sim)
self.args = args
self.root = None
self.camera_pose = camera_pose
if not no_graphics:
self._add_rendering(mode='osg')
# TODO: Is this needed?
self.fps = int(np.round(1.0 / self.dt))
self.np_random = None
self.seed()
self.n_actions = n_actions
self.observation_type = observation_type
if not no_graphics:
self._render_callback()
obs = self._get_observation()
self.observation_space = spaces.Box(low=-1, high=1, shape=(obs.shape), dtype=np.float32)
self.action_space = spaces.Box(-1., 1., shape=(self.n_actions,), dtype='float32')
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__(self, **kwargs):
"""
Construct given optional arguments.
Arguments:
driveline_settings: Struct - driveline settings, ExcavatorCAT365.default_driveline_settings() is used if not given.
track_settings: Struct - track settings, ExcavatorCAT365.default_track_settings() is used if not given.
keyboard_controls: Struct - keyboard controls (e.g., ExcavatorCAT365.default_keyboard_settings())
gamepad_controls: Struct - gamepad controls (e.g., ExcavatorCAT365.default_gamepad_controls())
"""
super().__init__()
temp_simulation = agxSDK.Simulation()
data = SimulationFile( simulation = temp_simulation, root = root() )
if not data.load( filename = self.model_path, parent = self ):
raise FileNotFoundError( 'Unable to load excavator model: {}'.format( self.model_path ) )
states = []
for rb in self.getRigidBodies():
states.append( rb.getEnable() )
disabled_collisions = DisabledCollisionsStateHandler( temp_simulation )
for disabled in disabled_collisions:
disabled1 = disabled[ 0 ]
disabled2 = disabled[ 1 ] if len( disabled ) > 1 else disabled[ 0 ]
if isinstance( disabled1, agxCollide.Geometry ):
continue
global_simulation().getSpace().setEnablePair( disabled1, disabled2, False )
self._initialize( data, **kwargs )
del disabled_collisions
del data
del temp_simulation
for i, rb in enumerate( self.getRigidBodies() ):
rb.setEnable( states[ i ] )
def build_simulation():
# Instantiate a simulation
sim = agxSDK.Simulation()
# By default the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that
# too by creating an agx.PointGravityField for example).
# AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen)
if not GRAVITY:
logger.info("Gravity off.")
g = agx.Vec3(0, 0, 0) # remove gravity
sim.setUniformGravity(g)
# Get current delta-t (timestep) that is used in the simulation?
dt = sim.getTimeStep()
logger.debug("default dt = {}".format(dt))
# Change the timestep
sim.setTimeStep(TIMESTEP)
# Confirm timestep changed
dt = sim.getTimeStep()
logger.debug("new dt = {}".format(dt))
# Create a ground plane for reference
ground = create_body(name="ground",
shape=agxCollide.Box(CYLINDER_LENGTH * 2,
CYLINDER_LENGTH * 2,
GROUND_WIDTH),
position=agx.Vec3(0, 0, 0),
motion_control=agx.RigidBody.STATIC)
sim.add(ground)
rotation_cylinder = agx.OrthoMatrix3x3()
rotation_cylinder.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.Z_AXIS())
material_cylinder = agx.Material("Aluminum")
bulk_material_cylinder = material_cylinder.getBulkMaterial()
bulk_material_cylinder.setPoissonsRatio(ALUMINUM_POISSON_RATIO)
bulk_material_cylinder.setYoungsModulus(ALUMINUM_YOUNG_MODULUS)
# Create cylinders
bottom_cylinder = create_body(
name="bottom_cylinder",
shape=agxCollide.Cylinder(CYLINDER_RADIUS, 3 / 4 * CYLINDER_LENGTH),
position=agx.Vec3(0, 0, GROUND_WIDTH + (3 / 4 * CYLINDER_LENGTH) / 2),
rotation=rotation_cylinder,
motion_control=agx.RigidBody.STATIC,
material=material_cylinder)
sim.add(bottom_cylinder)
middle_cylinder = create_body(
name="middle_cylinder",
shape=agxCollide.Cylinder(CYLINDER_RADIUS - GROOVE_DEPTH,
GROOVE_WIDTH),
position=agx.Vec3(
0, 0, GROUND_WIDTH + (3 / 4 * CYLINDER_LENGTH) + GROOVE_WIDTH / 2),
rotation=rotation_cylinder,
motion_control=agx.RigidBody.STATIC,
material=material_cylinder)
sim.add(middle_cylinder)
top_cylinder = create_body(
name="top_cylinder",
shape=agxCollide.Cylinder(CYLINDER_RADIUS, 1 / 4 * CYLINDER_LENGTH),
position=agx.Vec3(
0, 0, GROUND_WIDTH + (3 / 4 * CYLINDER_LENGTH) + GROOVE_WIDTH +
(1 / 4 * CYLINDER_LENGTH) / 2),
rotation=rotation_cylinder,
motion_control=agx.RigidBody.STATIC,
material=material_cylinder)
sim.add(top_cylinder)
material_ring = agx.Material("Rubber")
bulk_material_ring = material_ring.getBulkMaterial()
bulk_material_ring.setPoissonsRatio(RUBBER_POISSON_RATIO)
bulk_material_ring.setYoungsModulus(RUBBER_YOUNG_MODULUS)
ring = create_ring(name="ring",
radius=RING_RADIUS,
element_shape=agxCollide.Capsule(
RING_CROSS_SECTION, RING_SEGMENT_LENGTH),
num_elements=NUM_RING_ELEMENTS,
constraint_type=agx.LockJoint,
rotation_shift=math.pi / 2,
translation_shift=RING_SEGMENT_LENGTH / 2,
compliance=RING_COMPLIANCE,
center=agx.Vec3(0, 0, CYLINDER_LENGTH + RING_RADIUS),
material=material_ring)
sim.add(ring)
left_ring_element = sim.getRigidBody(LEFT_ELEMENT)
right_ring_element = sim.getRigidBody(RIGHT_ELEMENT)
gripper_left = create_body(
name="gripper_left",
shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER, SIZE_GRIPPER),
position=left_ring_element.getPosition(),
rotation=agx.OrthoMatrix3x3(left_ring_element.getRotation()),
motion_control=agx.RigidBody.DYNAMICS)
sim.add(gripper_left)
gripper_right = create_body(
name="gripper_right",
shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER, SIZE_GRIPPER),
position=right_ring_element.getPosition(),
rotation=agx.OrthoMatrix3x3(right_ring_element.getRotation()),
motion_control=agx.RigidBody.DYNAMICS)
sim.add(gripper_right)
# Disable collisions for grippers
gripper_left_body = sim.getRigidBody("gripper_left")
gripper_left_body.getGeometry("gripper_left").setEnableCollisions(False)
gripper_right_body = sim.getRigidBody("gripper_right")
gripper_right_body.getGeometry("gripper_right").setEnableCollisions(False)
frame_element = agx.Frame()
frame_gripper = agx.Frame()
result = agx.Constraint.calculateFramesFromBody(
agx.Vec3(RING_CROSS_SECTION, 0, 0), agx.Vec3(1, 0, 0),
left_ring_element, frame_element, gripper_left_body, frame_gripper)
print(result)
lock_joint_left = agx.LockJoint(gripper_left_body, frame_gripper,
left_ring_element, frame_element)
lock_joint_left.setName('lock_joint_left')
lock_joint_left.setCompliance(GRIPPER_COMPLIANCE)
sim.add(lock_joint_left)
frame_gripper = agx.Frame()
result = agx.Constraint.calculateFramesFromBody(
agx.Vec3(RING_CROSS_SECTION, 0, 0), agx.Vec3(1, 0, 0),
right_ring_element, frame_element, gripper_right_body, frame_gripper)
print(result)
lock_joint_right = agx.LockJoint(gripper_right_body, frame_gripper,
right_ring_element, frame_element)
lock_joint_right.setName('lock_joint_right')
lock_joint_right.setCompliance(GRIPPER_COMPLIANCE)
sim.add(lock_joint_right)
# Create contact materials
contact_material = sim.getMaterialManager().getOrCreateContactMaterial(
material_cylinder, material_ring)
contact_material.setYoungsModulus(CONTACT_YOUNG_MODULUS)
# Create friction model, DIRECT is more accurate, but slower
# fm = agx.IterativeProjectedConeFriction()
# fm.setSolveType(agx.FrictionModel.DIRECT)
# contact_material.setFrictionModel(fm)
# Create bases for gripper motors
prismatic_base_left = create_hinge_prismatic_base(
"gripper_left",
gripper_left_body,
position_ranges=[(-CYLINDER_RADIUS, CYLINDER_RADIUS),
(-CYLINDER_RADIUS, CYLINDER_RADIUS),
(-(CYLINDER_LENGTH + 2 * RING_RADIUS), RING_RADIUS),
(-math.pi / 4, math.pi / 4)])
sim.add(prismatic_base_left)
prismatic_base_right = create_hinge_prismatic_base(
"gripper_right",
gripper_right_body,
position_ranges=[(-CYLINDER_RADIUS, CYLINDER_RADIUS),
(-CYLINDER_RADIUS, CYLINDER_RADIUS),
(-CYLINDER_LENGTH - RING_RADIUS, RING_RADIUS),
(-math.pi / 4, math.pi / 4)])
sim.add(prismatic_base_right)
# Add keyboard listener
left_motor_x = sim.getConstraint1DOF(
"gripper_left_joint_base_x").getMotor1D()
left_motor_y = sim.getConstraint1DOF(
"gripper_left_joint_base_y").getMotor1D()
left_motor_z = sim.getConstraint1DOF(
"gripper_left_joint_base_z").getMotor1D()
left_motor_rb = sim.getConstraint1DOF("gripper_left_joint_rb").getMotor1D()
right_motor_x = sim.getConstraint1DOF(
"gripper_right_joint_base_x").getMotor1D()
right_motor_y = sim.getConstraint1DOF(
"gripper_right_joint_base_y").getMotor1D()
right_motor_z = sim.getConstraint1DOF(
"gripper_right_joint_base_z").getMotor1D()
right_motor_rb = sim.getConstraint1DOF(
"gripper_right_joint_rb").getMotor1D()
key_motor_map = {
agxSDK.GuiEventListener.KEY_Right: (right_motor_x, 0.1),
agxSDK.GuiEventListener.KEY_Left: (right_motor_x, -0.1),
agxSDK.GuiEventListener.KEY_Up: (right_motor_y, 0.1),
agxSDK.GuiEventListener.KEY_Down: (right_motor_y, -0.1),
65365: (right_motor_z, 0.1),
65366: (right_motor_z, -0.1),
0x64: (left_motor_x, 0.1),
0x61: (left_motor_x, -0.1),
0x32: (left_motor_y,
0.1), # 0x77 W is replaced with 2, due to prior shortcut
0x73: (left_motor_y, -0.1),
0x71: (left_motor_z, 0.1),
0x65: (left_motor_z, -0.1),
0x72: (left_motor_rb, 0.1), # R
0x74: (left_motor_rb, -0.1), # T
0x6f: (right_motor_rb, 0.1), # O
0x70: (right_motor_rb, -0.1)
} # P
sim.add(KeyboardMotorHandler(key_motor_map))
return sim
def build_simulation():
# Instantiate a simulation
sim = agxSDK.Simulation()
# By default the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that
# too by creating an agx.PointGravityField for example).
# AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen)
if not GRAVITY:
logger.info("Gravity off.")
g = agx.Vec3(0, 0, 0) # remove gravity
sim.setUniformGravity(g)
# Get current delta-t (timestep) that is used in the simulation?
dt = sim.getTimeStep()
logger.debug("default dt = {}".format(dt))
# Change the timestep
sim.setTimeStep(TIMESTEP)
# Confirm timestep changed
dt = sim.getTimeStep()
logger.debug("new dt = {}".format(dt))
# Define materials
material_hard = agx.Material("Aluminum")
material_hard_bulk = material_hard.getBulkMaterial()
material_hard_bulk.setPoissonsRatio(ALUMINUM_POISSON_RATIO)
material_hard_bulk.setYoungsModulus(ALUMINUM_YOUNG_MODULUS)
# Create gripper
create_gripper_peg_in_hole(sim=sim,
name="gripper",
material=material_hard,
position=agx.Vec3(0.0, 0.0, 0.35),
geometry_transform=agx.AffineMatrix4x4(),
joint_ranges=JOINT_RANGES,
force_ranges=FORCE_RANGES)
# Create hollow cylinde with hole
scaling_cylinder = agx.Matrix3x3(agx.Vec3(0.0275))
hullMesh = agxUtil.createTrimeshFromWavefrontOBJ(MESH_HOLLOW_CYLINDER_FILE,
0, scaling_cylinder)
hullGeom = agxCollide.Geometry(
hullMesh,
agx.AffineMatrix4x4.rotate(agx.Vec3(0, 1, 0), agx.Vec3(0, 0, 1)))
hollow_cylinder = agx.RigidBody("hollow_cylinder")
hollow_cylinder.add(hullGeom)
hollow_cylinder.setMotionControl(agx.RigidBody.STATIC)
hullGeom.setMaterial(material_hard)
sim.add(hollow_cylinder)
# Create rope and set name + properties
peg = agxCable.Cable(RADIUS, RESOLUTION)
peg.setName("DLO")
material_peg = peg.getMaterial()
peg_material = material_peg.getBulkMaterial()
peg_material.setPoissonsRatio(PEG_POISSON_RATIO)
properties = peg.getCableProperties()
properties.setYoungsModulus(YOUNG_MODULUS_BEND, agxCable.BEND)
properties.setYoungsModulus(YOUNG_MODULUS_TWIST, agxCable.TWIST)
properties.setYoungsModulus(YOUNG_MODULUS_STRETCH, agxCable.STRETCH)
# Add connection between cable and gripper
tf_0 = agx.AffineMatrix4x4()
tf_0.setTranslate(0.0, 0, 0.075)
peg.add(agxCable.BodyFixedNode(sim.getRigidBody("gripper_body"), tf_0))
peg.add(agxCable.FreeNode(0.0, 0.0, 0.1))
sim.add(peg)
segment_iterator = peg.begin()
n_segments = peg.getNumSegments()
for i in range(n_segments):
if not segment_iterator.isEnd():
seg = segment_iterator.getRigidBody()
seg.setAngularVelocityDamping(1e3)
segment_iterator.inc()
# Try to initialize rope
report = peg.tryInitialize()
if report.successful():
print("Successful rope initialization.")
else:
print(report.getActualError())
# Add rope to simulation
sim.add(peg)
# Set rope material
material_peg = peg.getMaterial()
material_peg.setName("rope_material")
contactMaterial = sim.getMaterialManager().getOrCreateContactMaterial(
material_hard, material_peg)
contactMaterial.setYoungsModulus(1e12)
fm = agx.IterativeProjectedConeFriction()
fm.setSolveType(agx.FrictionModel.DIRECT)
contactMaterial.setFrictionModel(fm)
# Add keyboard listener
motor_x = sim.getConstraint1DOF("gripper_joint_base_x").getMotor1D()
motor_y = sim.getConstraint1DOF("gripper_joint_base_y").getMotor1D()
motor_z = sim.getConstraint1DOF("gripper_joint_base_z").getMotor1D()
motor_rot_y = sim.getConstraint1DOF("gripper_joint_rot_y").getMotor1D()
key_motor_map = {
agxSDK.GuiEventListener.KEY_Up: (motor_y, 0.5),
agxSDK.GuiEventListener.KEY_Down: (motor_y, -0.5),
agxSDK.GuiEventListener.KEY_Right: (motor_x, 0.5),
agxSDK.GuiEventListener.KEY_Left: (motor_x, -0.5),
65365: (motor_z, 0.5),
65366: (motor_z, -0.5),
120: (motor_rot_y, 5),
121: (motor_rot_y, -5)
}
sim.add(KeyboardMotorHandler(key_motor_map))
rbs = peg.getRigidBodies()
for i in range(len(rbs)):
rbs[i].setName('dlo_' + str(i + 1))
return sim
def build_simulation(goal=False):
"""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
def build_simulation(goal=False):
"""Builds simulations for both start and goal configurations
:param bool goal: toggles between simulation definition of start and goal configurations
:return agxSDK.Simulation: simulation object
"""
assembly_name = "start_"
goal_string = ""
if goal:
assembly_name = "goal_"
goal_string = "_goal"
# Instantiate a simulation
sim = agxSDK.Simulation()
# By default, the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that
# too by creating an agx.PointGravityField for example).
# AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen)
if not GRAVITY:
logger.info("Gravity off.")
g = agx.Vec3(0, 0, 0) # remove gravity
sim.setUniformGravity(g)
# Get current delta-t (timestep) that is used in the simulation?
dt = sim.getTimeStep()
logger.debug("default dt = {}".format(dt))
# Change the timestep
sim.setTimeStep(TIMESTEP)
# Confirm timestep changed
dt = sim.getTimeStep()
logger.debug("new dt = {}".format(dt))
# Create a new empty Assembly
scene = agxSDK.Assembly()
scene.setName(assembly_name + "assembly")
# Add start assembly to simulation
sim.add(scene)
# Create a ground plane for reference and obstacle
if not goal:
ground = create_body(name="ground",
shape=agxCollide.Box(LENGTH, LENGTH,
GROUND_WIDTH),
position=agx.Vec3(
0, 0,
-(GROUND_WIDTH + SIZE_GRIPPER / 2 + LENGTH)),
motion_control=agx.RigidBody.STATIC)
sim.add(ground)
# Create two grippers
gripper_left = create_body(name="gripper_left" + goal_string,
shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER,
SIZE_GRIPPER),
position=agx.Vec3(-LENGTH / 2, 0, 0),
motion_control=agx.RigidBody.DYNAMICS)
scene.add(gripper_left)
gripper_right = create_body(name="gripper_right" + goal_string,
shape=agxCollide.Box(SIZE_GRIPPER,
SIZE_GRIPPER,
SIZE_GRIPPER),
position=agx.Vec3(LENGTH / 2, 0, 0),
motion_control=agx.RigidBody.DYNAMICS)
scene.add(gripper_right)
gripper_left_body = gripper_left.getRigidBody("gripper_left" + goal_string)
gripper_right_body = gripper_right.getRigidBody("gripper_right" +
goal_string)
# Create material
material_cylinder = agx.Material("cylinder_material")
bulk_material_cylinder = material_cylinder.getBulkMaterial()
bulk_material_cylinder.setPoissonsRatio(POISSON_RATIO)
bulk_material_cylinder.setYoungsModulus(YOUNG_MODULUS)
cylinder = create_body(name="obstacle" + goal_string,
shape=agxCollide.Cylinder(CYLINDER_RADIUS,
CYLINDER_LENGTH),
position=agx.Vec3(0, 0, -2 * CYLINDER_RADIUS),
motion_control=agx.RigidBody.STATIC,
material=material_cylinder)
scene.add(cylinder)
# Create cable
cable = agxCable.Cable(RADIUS, RESOLUTION)
# Create Frames for each gripper:
# Cables are attached passing through the attachment point along the Z axis of the body's coordinate frame.
# The translation specified in the transformation is relative to the body and not the world
left_transform = agx.AffineMatrix4x4()
left_transform.setTranslate(SIZE_GRIPPER + RADIUS, 0, 0)
left_transform.setRotate(
agx.Vec3.Z_AXIS(),
agx.Vec3.Y_AXIS()) # Rotation matrix which switches Z with Y
frame_left = agx.Frame(left_transform)
right_transform = agx.AffineMatrix4x4()
right_transform.setTranslate(-SIZE_GRIPPER - RADIUS, 0, 0)
right_transform.setRotate(
agx.Vec3.Z_AXIS(),
-agx.Vec3.Y_AXIS()) # Rotation matrix which switches Z with -Y
frame_right = agx.Frame(right_transform)
cable.add(
agxCable.FreeNode(agx.Vec3(-LENGTH / 2 + SIZE_GRIPPER + RADIUS, 0,
0))) # Fix cable to gripper_left
cable.add(
agxCable.FreeNode(agx.Vec3(LENGTH / 2 - SIZE_GRIPPER - RADIUS, 0,
0))) # Fix cable to gripper_right
# Set cable name and properties
cable.setName("DLO" + goal_string)
properties = cable.getCableProperties()
properties.setYoungsModulus(YOUNG_MODULUS, agxCable.BEND)
properties.setYoungsModulus(YOUNG_MODULUS, agxCable.TWIST)
properties.setYoungsModulus(YOUNG_MODULUS, agxCable.STRETCH)
material_wire = cable.getMaterial()
wire_material = material_wire.getBulkMaterial()
wire_material.setPoissonsRatio(POISSON_RATIO)
wire_material.setYoungsModulus(YOUNG_MODULUS)
cable.setMaterial(material_wire)
# Add cable plasticity
plasticity = agxCable.CablePlasticity()
plasticity.setYieldPoint(
YIELD_POINT,
agxCable.BEND) # set torque required for permanent deformation
plasticity.setYieldPoint(
YIELD_POINT,
agxCable.STRETCH) # set torque required for permanent deformation
cable.addComponent(plasticity) # NOTE: Stretch direction is always elastic
# Tell MaterialManager to create and return a contact material which will be used
# when two geometries both with this material is in contact
contact_material = sim.getMaterialManager().getOrCreateContactMaterial(
material_cylinder, material_wire)
contact_material.setYoungsModulus(CONTACT_YOUNG_MODULUS)
# Create a Friction model, which we tell the solver to solve ITERATIVELY (faster)
fm = agx.IterativeProjectedConeFriction()
fm.setSolveType(agx.FrictionModel.DIRECT)
contact_material.setFrictionModel(fm)
# Try to initialize cable
report = cable.tryInitialize()
if report.successful():
logger.debug("Successful cable initialization.")
else:
logger.error(report.getActualError())
# Add cable to simulation
sim.add(cable)
# Add segment names and get first and last segment
segment_count = 0
iterator = cable.begin()
segment_left = iterator.getRigidBody()
segment_left.setName('dlo_' + str(segment_count + 1) + goal_string)
segment_right = None
while not iterator.isEnd():
segment_count += 1
segment_right = iterator.getRigidBody()
segment_right.setName('dlo_' + str(segment_count + 1) + goal_string)
iterator.inc()
# Add hinge constraints
hinge_joint_left = agx.Hinge(
sim.getRigidBody("gripper_left" + goal_string), frame_left,
segment_left)
hinge_joint_left.setName('hinge_joint_left' + goal_string)
motor_left = hinge_joint_left.getMotor1D()
motor_left.setEnable(False)
motor_left_param = motor_left.getRegularizationParameters()
motor_left_param.setCompliance(1e12)
motor_left.setLockedAtZeroSpeed(False)
lock_left = hinge_joint_left.getLock1D()
lock_left.setEnable(False)
range_left = hinge_joint_left.getRange1D()
range_left.setEnable(True)
range_left.setRange(agx.RangeReal(-math.pi / 2, math.pi / 2))
sim.add(hinge_joint_left)
hinge_joint_right = agx.Hinge(
sim.getRigidBody("gripper_right" + goal_string), frame_right,
segment_right)
hinge_joint_right.setName('hinge_joint_right' + goal_string)
motor_right = hinge_joint_right.getMotor1D()
motor_right.setEnable(False)
motor_right_param = motor_right.getRegularizationParameters()
motor_right_param.setCompliance(1e12)
motor_right.setLockedAtZeroSpeed(False)
lock_right = hinge_joint_right.getLock1D()
lock_right.setEnable(False)
range_right = hinge_joint_right.getRange1D()
range_right.setEnable(True)
range_right.setRange(agx.RangeReal(-math.pi / 2, math.pi / 2))
sim.add(hinge_joint_right)
# Create bases for gripper motors
prismatic_base_left = create_locked_prismatic_base(
"gripper_left" + goal_string,
gripper_left_body,
compliance=0,
motor_ranges=[(-FORCE_RANGE, FORCE_RANGE), (-FORCE_RANGE, FORCE_RANGE),
(-FORCE_RANGE, FORCE_RANGE)],
position_ranges=[(-LENGTH / 2 + CYLINDER_RADIUS,
LENGTH / 2 - CYLINDER_RADIUS),
(-CYLINDER_LENGTH / 3, CYLINDER_LENGTH / 3),
(-(GROUND_WIDTH + SIZE_GRIPPER / 2 + LENGTH), 0)],
lock_status=[False, False, False])
sim.add(prismatic_base_left)
prismatic_base_right = create_locked_prismatic_base(
"gripper_right" + goal_string,
gripper_right_body,
compliance=0,
motor_ranges=[(-FORCE_RANGE, FORCE_RANGE), (-FORCE_RANGE, FORCE_RANGE),
(-FORCE_RANGE, FORCE_RANGE)],
position_ranges=[(-LENGTH / 2 + CYLINDER_RADIUS,
LENGTH / 2 - CYLINDER_RADIUS),
(-CYLINDER_LENGTH / 3, CYLINDER_LENGTH / 3),
(-(GROUND_WIDTH + SIZE_GRIPPER / 2 + LENGTH), 0)],
lock_status=[False, False, False])
sim.add(prismatic_base_right)
return sim
def build_simulation():
# Instantiate a simulation
sim = agxSDK.Simulation()
# By default the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that
# too by creating an agx.PointGravityField for example).
# AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen)
if not GRAVITY:
logger.info("Gravity off.")
g = agx.Vec3(0, 0, 0) # remove gravity
sim.setUniformGravity(g)
# Get current delta-t (timestep) that is used in the simulation?
dt = sim.getTimeStep()
logger.debug("default dt = {}".format(dt))
# Change the timestep
sim.setTimeStep(TIMESTEP)
# Confirm timestep changed
dt = sim.getTimeStep()
logger.debug("new dt = {}".format(dt))
# Define materials
material_hard = agx.Material("Aluminum")
material_hard_bulk = material_hard.getBulkMaterial()
material_hard_bulk.setPoissonsRatio(ALUMINUM_POISSON_RATIO)
material_hard_bulk.setYoungsModulus(ALUMINUM_YOUNG_MODULUS)
# Create a ground plane
ground = create_body(name="ground",
shape=agxCollide.Box(GROUND_LENGTH_X, GROUND_LENGTH_Y,
GROUND_HEIGHT),
position=agx.Vec3(0, 0, -0.005),
motion_control=agx.RigidBody.STATIC)
sim.add(ground)
# Creates poles
for i, position in enumerate(POLE_POSITION_OFFSETS):
create_pole(id=i, sim=sim, position=position, material=material_hard)
# Create gripper
gripper = create_body(name="gripper",
shape=agxCollide.Sphere(0.002),
position=agx.Vec3(0.0, 0.0,
GRIPPER_HEIGHT + DIAMETER / 2.0),
motion_control=agx.RigidBody.DYNAMICS,
material=material_hard)
gripper.getRigidBody("gripper").getGeometry("gripper").setEnableCollisions(
False)
sim.add(gripper)
# Create base for pusher motors
prismatic_base = create_locked_prismatic_base(
"gripper",
gripper.getRigidBody("gripper"),
position_ranges=[(-GRIPPER_MAX_X, GRIPPER_MAX_X),
(-GRIPPER_MAX_Y, GRIPPER_MAX_Y),
(GRIPPER_MIN_Z, GRIPPER_MAX_Z)],
motor_ranges=[(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE)])
sim.add(prismatic_base)
# Create rope and set name + properties
rubber_band = agxCable.Cable(RADIUS, RESOLUTION)
rubber_band.setName("DLO")
material_rubber_band = rubber_band.getMaterial()
rubber_band_material = material_rubber_band.getBulkMaterial()
rubber_band_material.setPoissonsRatio(PEG_POISSON_RATIO)
properties = rubber_band.getCableProperties()
properties.setYoungsModulus(YOUNG_MODULUS_BEND, agxCable.BEND)
properties.setYoungsModulus(YOUNG_MODULUS_TWIST, agxCable.TWIST)
properties.setYoungsModulus(YOUNG_MODULUS_STRETCH, agxCable.STRETCH)
# Initialize dlo on circle
steps = DLO_CIRCLE_STEPS
for a in np.linspace(-np.pi / 2, (3.0 / 2.0) * np.pi - 2 * np.pi / steps,
steps):
x = np.cos(a) * DIAMETER / 2.0
z = np.sin(a) * DIAMETER / 2.0
rubber_band.add(agxCable.FreeNode(x, 0, GRIPPER_HEIGHT + z))
sim.add(rubber_band)
segments_cable = list()
cable = agxCable.Cable.find(sim, "DLO")
segment_iterator = cable.begin()
n_segments = cable.getNumSegments()
for i in range(n_segments):
if not segment_iterator.isEnd():
seg = segment_iterator.getRigidBody()
seg.setAngularVelocityDamping(1e4)
mass_props = seg.getMassProperties()
mass_props.setMass(1.25 * mass_props.getMass())
segments_cable.append(seg)
segment_iterator.inc()
# Get segments at ends and middle
s0 = segments_cable[0]
s1 = segments_cable[int(n_segments / 2)]
s2 = segments_cable[-1]
# Add ball joint between gripper and rubber band
f0 = agx.Frame()
f1 = agx.Frame()
ball_joint = agx.BallJoint(gripper.getRigidBody("gripper"), f0, s1, f1)
sim.add(ball_joint)
# Connect ends of rubber band
f0 = agx.Frame()
f0.setLocalTranslate(0.0, 0.0,
-1 * np.pi * DIAMETER / cable.getNumSegments())
f1 = agx.Frame()
lock = agx.LockJoint(s0, f0, s2, f1)
lock.setCompliance(1.0e-4)
sim.add(lock)
# Try to initialize dlo
report = rubber_band.tryInitialize()
if report.successful():
print("Successful dlo initialization.")
else:
print(report.getActualError())
# Add rope to simulation
sim.add(rubber_band)
# Set rope material
material_rubber_band = rubber_band.getMaterial()
material_rubber_band.setName("rope_material")
contactMaterial = sim.getMaterialManager().getOrCreateContactMaterial(
material_hard, material_rubber_band)
contactMaterial.setYoungsModulus(1e12)
fm = agx.IterativeProjectedConeFriction()
fm.setSolveType(agx.FrictionModel.DIRECT)
contactMaterial.setFrictionModel(fm)
# Add keyboard listener
motor_x = sim.getConstraint1DOF("gripper_joint_base_x").getMotor1D()
motor_y = sim.getConstraint1DOF("gripper_joint_base_y").getMotor1D()
motor_z = sim.getConstraint1DOF("gripper_joint_base_z").getMotor1D()
key_motor_map = {
agxSDK.GuiEventListener.KEY_Up: (motor_y, 0.2),
agxSDK.GuiEventListener.KEY_Down: (motor_y, -0.2),
agxSDK.GuiEventListener.KEY_Right: (motor_x, 0.2),
agxSDK.GuiEventListener.KEY_Left: (motor_x, -0.2),
65365: (motor_z, 0.2),
65366: (motor_z, -0.2)
}
sim.add(KeyboardMotorHandler(key_motor_map))
rbs = rubber_band.getRigidBodies()
for i in range(len(rbs)):
rbs[i].setName('dlo_' + str(i + 1))
return sim
def main():
ap = argparse.ArgumentParser()
ap.add_argument(
"input",
type=str,
help=
".agx file whos particleEmitters will be replaced with rigidBodyEmitters."
)
ap.add_argument("-o",
"--output",
default="output.agx",
type=str,
help="Path for the resulting .agx file.")
ap.add_argument("--shapeLibrary",
type=str,
default="",
help="Path to the shape library.")
args = vars(ap.parse_args())
input_path = args["input"]
output_path = args["output"]
shape_library_path = args["shapeLibrary"]
if os.path.isfile(input_path):
print("The serialized simulation to load:", input_path)
else:
sys.exit("Could not find input path:", input_path)
print("The path where the new simulation is saved:",
os.path.abspath(output_path))
# check that the shape library path exists
if not shape_library_path:
if os.environ.get("AGX_DATA_DIR"):
shape_library_path = os.path.join(os.environ.get("AGX_DATA_DIR"),
"models\\convex_stones")
else:
sys.exit("No shape library found or given.")
if os.path.isdir(shape_library_path):
print("The path to the shape library is:", shape_library_path)
else:
sys.exit("Shape library path do not exist, or is not a directory:",
shape_library_path)
print(
"------------------------------------------------------------------------------"
)
shape_paths = find_shapes(shape_library_path)
if len(shape_paths) < 1:
sys.exit("Did not find any shapes in ", shape_library_path)
sim = agxSDK.Simulation()
sim.read(input_path, None, agxSDK.Simulation.READ_NONE)
print(
"------------------------------------------------------------------------------"
)
particle_emitters = sim.getParticleEmitters()
if particle_emitters is None:
sys.exit("Did not find any particle emitters in simulation.")
else:
print(
"Found %d particle emitters. Replacing with rigidbody emitters ..."
% len(particle_emitters))
existing_materials = {}
geoms = sim.getGeometries()
for g in geoms:
m = g.getMaterial()
if m not in existing_materials.values():
existing_materials[m.getName()] = m
# Replace each of the particle emitters with rigid body emitters
particle_model_materials = []
rb_model_materials = []
for pe in particle_emitters:
dt = pe.getDistributionTable()
# get emitter geometry
geom = pe.getGeometry()
geom.setSensor(True)
# get the size, material and probability weight of all particle models
weights = []
sizes = []
models = dt.getModels()
for pm in models:
m = pm.asParticleEmitterDistributionModel()
sizes.append(m.getParticleRadius() * 2.0)
weights.append(m.getProbabilityWeight())
material = m.getParticleMaterial()
if len(sizes) < 1:
sys.exit("No particle models set on particle emitter",
geom.getName())
# create a new material to use for the rigidbodies
particle_model_materials.append(material)
rb_mat = create_rigid_body_material_from_particle_material(material)
rb_model_materials.append(rb_mat)
# create rigid body distribution table from shape library
dt_rb = create_distribution_table(shape_paths, sizes, weights, rb_mat)
dt_rb.setProbabilityQuantity(dt.getProbabilityQuantity())
# create rigidbody emitter
rbe = agx.RigidBodyEmitter()
rbe.setQuantity(pe.getQuantity())
rbe.setRate(pe.getRate())
rbe.setMaximumEmittedQuantity(pe.getMaximumEmittedQuantity())
rbe.setGeometry(geom)
rbe.setDistributionTable(dt_rb)
# switch the emitter type in the simulation
sim.add(rbe)
sim.remove(pe)
print("Replaced particle emitter on geometry: %s" % geom.getName())
print(
"------------------------------------------------------------------------------"
)
print(
"Copying contact material properties from old particle material to the new rb material "
)
mm = sim.getMaterialManager()
iterative_projected_cone_friction = agx.IterativeProjectedConeFriction()
iterative_projected_cone_friction.setSolveType(agx.FrictionModel.ITERATIVE)
# Copy material properties from the old particle material to the new rigidbody material
def copy_cm_properties(old_pair, new_pair):
cm0 = mm.getContactMaterial(old_pair[0], old_pair[1])
restitution = cm0.getRestitution()
friction = cm0.getFrictionCoefficient()
cm1 = mm.getOrCreateContactMaterial(new_pair[0], new_pair[1])
cm1.setRestitution(restitution)
cm1.setFrictionCoefficient(friction)
return cm1
i = 0
for particle_material, rb_material in zip(particle_model_materials,
rb_model_materials):
for k, v in existing_materials.items():
cm1 = copy_cm_properties((particle_material, v), (v, rb_material))
print("Material properties for contact material: %s-%s" %
(k, rb_material.getName()))
print("... friction coefficient is: ",
cm1.getFrictionCoefficient())
print("... restitution is: ", cm1.getRestitution())
for particle_material2, rb_material2 in zip(
particle_model_materials[i:], rb_model_materials[i:]):
cm1 = copy_cm_properties((particle_material, particle_material2),
(rb_material, rb_material2))
cm1.setFrictionModel(iterative_projected_cone_friction)
print("Material properties for contact material: %s-%s" %
(rb_material.getName(), rb_material2.getName()))
print("... friction coefficient is: ",
cm1.getFrictionCoefficient())
print("... restitution is: ", cm1.getRestitution())
i += 1
print(
"------------------------------------------------------------------------------"
)
print("Removing all the particle materials...")
# Remove all the old particle materials
for pmat in particle_model_materials:
print("... removing material \'%s\'" % pmat.getName())
sim.getMaterialManager().remove(pmat)
print(
"------------------------------------------------------------------------------"
)
print("Finding particle sinks and making them rigidBody sinks...")
# Find all the rigidbodies that are named "Sinki" for i=0...n where n+1 returns None.
# and set a sensoroperation to remove rigidBodies.
i = 0
while True:
rb = sim.getRigidBody("Sink%d" % i)
if rb is not None:
for g in rb.getGeometries():
if g.isSensor():
print(
"... Found sink geometry \'%s\'. Is now rigidBody sink aswell."
% g.getName())
event_sensor = agxControl.EventSensor(g)
sensor_op = agxControl.RemoveRigidBody()
event_sensor.addSensorEvent(sensor_op)
sim.add(event_sensor)
else:
break
i += 1
print(
"------------------------------------------------------------------------------"
)
sim.write(os.path.abspath(output_path))
print("Saved simulation as:", os.path.abspath(output_path))
print("You can run the simulation with the command agxviewer %s" %
os.path.abspath(output_path))
print(
"Use the flags --journalRecord --journalIncrementalStructure to record a journal."
)
def __init__(self, scene_path, n_substeps, n_actions, observation_config,
camera_pose, osg_window, agx_only, args):
"""Initializes a AgxEnv object
:param str scene_path: path to binary file containing serialized simulation defined in sim/ folder
:param int n_substeps: number os simulation steps per call to step()
:param int n_actions: number of actions (DoF)
:param gym_agx.rl.observation.ObservationConfig observation_config: observation configuration object
:param dict camera_pose: dictionary containing EYE, CENTER, UP information for rendering
:param bool osg_window: enables/disables window rendering (useful for training)
:param bool agx_only: enables/disables all rendering (useful for training which does not use images)
:param list args: arguments for agxViewer
"""
self.render_mode = 'osg'
self.scene_path = scene_path
self.n_substeps = n_substeps
self.gravity = None
self.time_step = None
self.init = agx.AutoInit()
self.sim = agxSDK.Simulation()
self._build_simulation()
self.app = None
self.root = None
self.camera_pose = camera_pose
self.osg_window = osg_window
self.agx_only = agx_only
self.n_actions = n_actions
self.observation_config = observation_config
self.args = args
if not self.agx_only:
self.app = agxOSG.ExampleApplication(self.sim)
self.args = self.args + [
'--window', 2 * observation_config.image_size[1],
2 * observation_config.image_size[0]
]
if self.osg_window:
if self.observation_config.depth_in_obs or self.observation_config.rgb_in_obs:
warnings.warn((
"OSG window is enabled! \n"
"=======> Observations contain image data (OSG rendering cannot be disabled). \n"
"=======> Rendering is done inside step(). No need to call render()."
))
else:
warnings.warn((
"OSG window is enabled! \n"
"=======> Observations do not contain image data. \n"
"=======> Rendering is done inside render(), do not use 'human' or 'depth' mode."
))
else:
self.args = self.args + ['--osgWindow', '0']
if self.observation_config.depth_in_obs or self.observation_config.rgb_in_obs:
warnings.warn((
"OSG window is disabled! \n"
"=======> Observations contain image data (OSG rendering cannot be enabled). \n"
"=======> Rendering is done inside step(), "
"only 'human' or 'depth' modes available."))
else:
warnings.warn(
("OSG window is disabled! \n"
"=======> Observations do not contain image data. \n"
"=======> Rendering is done inside render(), "
"only 'human' or 'depth' modes available."))
self.render_to_image = []
self.img_object = None
# TODO: Is this needed?
self.fps = int(np.round(1.0 / self.dt))
self.np_random = None
self.seed()
self.goal = self._sample_goal()
obs = self._get_observation()
self.obs_space = construct_space(obs['observation'])
self.goal_space = construct_space(obs['desired_goal'])
self.action_space = spaces.Box(-1.,
1.,
shape=(self.n_actions, ),
dtype='float32')
self.observation_space = spaces.Dict(
dict(desired_goal=self.goal_space,
achieved_goal=self.goal_space,
observation=self.obs_space))
def build_simulation():
# Instantiate a simulation
sim = agxSDK.Simulation()
# By default the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that
# too by creating an agx.PointGravityField for example).
# AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen)
if not GRAVITY:
logger.info("Gravity off.")
g = agx.Vec3(0, 0, 0) # remove gravity
sim.setUniformGravity(g)
# Get current delta-t (timestep) that is used in the simulation?
dt = sim.getTimeStep()
logger.debug("default dt = {}".format(dt))
# Change the timestep
sim.setTimeStep(TIMESTEP)
# Confirm timestep changed
dt = sim.getTimeStep()
logger.debug("new dt = {}".format(dt))
# Define materials
material_hard = agx.Material("Aluminum")
material_hard_bulk = material_hard.getBulkMaterial()
material_hard_bulk.setPoissonsRatio(ALUMINUM_POISSON_RATIO)
material_hard_bulk.setYoungsModulus(ALUMINUM_YOUNG_MODULUS)
# Create box with pocket
side_length = 0.15
thickness_outer_wall = 0.01
body = create_body(name="ground", shape=agxCollide.Box(side_length, side_length, 0.01),
position=agx.Vec3(0, 0, -0.005),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(body)
body = create_body(name="walls", shape=agxCollide.Box(thickness_outer_wall, side_length, 0.04),
position=agx.Vec3(side_length + thickness_outer_wall, 0, 0.0),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(body)
body = create_body(name="walls", shape=agxCollide.Box(thickness_outer_wall, side_length, 0.04),
position=agx.Vec3(-(side_length + thickness_outer_wall), 0, 0.0),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(body)
body = create_body(name="walls",
shape=agxCollide.Box(side_length + 2 * thickness_outer_wall, thickness_outer_wall, 0.04),
position=agx.Vec3(0, -(side_length + thickness_outer_wall), 0.0),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(body)
body = create_body(name="walls",
shape=agxCollide.Box(side_length + 2 * thickness_outer_wall, thickness_outer_wall, 0.04),
position=agx.Vec3(0, side_length + thickness_outer_wall, 0.0),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(body)
# Create gripper 0
gripper_0 = create_body(name="gripper_0",
shape=agxCollide.Sphere(0.005),
position=agx.Vec3(-(LENGTH/2), OFFSET_Y, 0.0025),
motion_control=agx.RigidBody.DYNAMICS,
material=material_hard)
gripper_0.getRigidBody("gripper_0").getGeometry("gripper_0").setEnableCollisions(False)
sim.add(gripper_0)
# Create base for gripper motors
prismatic_base_0 = create_locked_prismatic_base("gripper_0", gripper_0.getRigidBody("gripper_0"),
position_ranges=[(-side_length*2, side_length*2),
(-side_length*2, side_length*2),
(-0.1, 0.01)],
motor_ranges=[(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE)],
lock_status=[False, False, False])
sim.add(prismatic_base_0)
# Create gripper
gripper_1 = create_body(name="gripper_1",
shape=agxCollide.Sphere(0.005),
position=agx.Vec3((LENGTH/2), OFFSET_Y, 0.0025),
motion_control=agx.RigidBody.DYNAMICS,
material=material_hard)
gripper_1.getRigidBody("gripper_1").getGeometry("gripper_1").setEnableCollisions(False)
sim.add(gripper_1)
# Create base for gripper motors
prismatic_base_0 = create_locked_prismatic_base("gripper_1", gripper_1.getRigidBody("gripper_1"),
position_ranges=[(-side_length*2, side_length*2),
(-side_length*2, side_length*2),
(-0.1, 0.01)],
motor_ranges=[(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE)],
lock_status=[False, False, False])
sim.add(prismatic_base_0)
# Create goal obstacle
goal_obstacle = create_body(name="obstacle_goal",
shape=agxCollide.Cylinder(2 * R_GOAL_OBSTACLE, 0.1),
position=agx.Vec3(0.0,0.-0.075, 0.005),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(goal_obstacle)
rotation_cylinder = agx.OrthoMatrix3x3()
rotation_cylinder.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.Z_AXIS())
goal_obstacle.setRotation(rotation_cylinder)
# Create obstacles
obs_pos = OBSTACLE_POSITIONS
for i in range(0, len(obs_pos)):
obstacle = create_body(name="obstacle",
shape=agxCollide.Box(0.01, 0.015, 0.05),
position=agx.Vec3(obs_pos[i][0], obs_pos[i][1], 0.005),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(obstacle)
# Create rope and set name + properties
dlo = agxCable.Cable(RADIUS, RESOLUTION)
dlo.setName("DLO")
material_rubber_band= dlo.getMaterial()
rubber_band_material = material_rubber_band.getBulkMaterial()
rubber_band_material.setPoissonsRatio(PEG_POISSON_RATIO)
properties = dlo.getCableProperties()
properties.setYoungsModulus(YOUNG_MODULUS_BEND, agxCable.BEND)
properties.setYoungsModulus(YOUNG_MODULUS_TWIST, agxCable.TWIST)
properties.setYoungsModulus(YOUNG_MODULUS_STRETCH, agxCable.STRETCH)
dlo.add(agxCable.FreeNode(gripper_0.getRigidBody("gripper_0").getPosition()))
dlo.add(agxCable.FreeNode(gripper_1.getRigidBody("gripper_1").getPosition()))
#
# hf = agx.HingeFrame()
# hf.setCenter(gripper_0.getRigidBody("gripper_0").getPosition())
# hf.setAxis(agx.Vec3(0,1,0))
# hinge_0 = agx.Hinge(hf, base_z, rot_y)
# agx.Hinge()
# dlo.add(agxCable.BodyFixedNode(gripper_0.getRigidBody("gripper_0"), agx.Vec3()))
# dlo.add(agxCable.BodyFixedNode(gripper_1.getRigidBody("gripper_1"), agx.Vec3()))
# Set angular damping for segments
sim.add(dlo)
segment_iterator = dlo.begin()
n_segments = dlo.getNumSegments()
segments = []
for i in range(n_segments):
if not segment_iterator.isEnd():
seg = segment_iterator.getRigidBody()
segments.append(seg)
seg.setAngularVelocityDamping(2e4)
segment_iterator.inc()
mass_props = seg.getMassProperties()
mass_props.setMass(1.25*mass_props.getMass())
s0 = segments[0]
s1 = segments[-1]
h0 = agx.HingeFrame()
h0.setCenter(gripper_0.getRigidBody("gripper_0").getPosition())
h0.setAxis(agx.Vec3(0,0,1))
l0 = agx.Hinge(h0, s0, gripper_0.getRigidBody("gripper_0") )
sim.add(l0)
h1 = agx.HingeFrame()
h1.setCenter(gripper_1.getRigidBody("gripper_1").getPosition())
h1.setAxis(agx.Vec3(0,0,1))
l1 = agx.Hinge(h1, s1, gripper_1.getRigidBody("gripper_1") )
sim.add(l1)
# f0 = agx.Frame()
# f1 = agx.Frame()
# l0 = agx.LockJoint(s0, f0, gripper_0.getRigidBody("gripper_0"), f1)
# l1 = agx.LockJoint(s1, f0, gripper_1.getRigidBody("gripper_1"), f1)
# sim.add(l0)
# sim.add(l1)
# Try to initialize dlo
report = dlo.tryInitialize()
if report.successful():
print("Successful dlo initialization.")
else:
print(report.getActualError())
# Add rope to simulation
sim.add(dlo)
# Set rope material
material_rubber_band = dlo.getMaterial()
material_rubber_band.setName("rope_material")
contactMaterial = sim.getMaterialManager().getOrCreateContactMaterial(material_hard, material_rubber_band)
contactMaterial.setYoungsModulus(1e12)
fm = agx.IterativeProjectedConeFriction()
fm.setSolveType(agx.FrictionModel.DIRECT)
contactMaterial.setFrictionModel(fm)
# Add keyboard listener
motor_x_0 = sim.getConstraint1DOF("gripper_0_joint_base_x").getMotor1D()
motor_y_0 = sim.getConstraint1DOF("gripper_0_joint_base_y").getMotor1D()
motor_x_1 = sim.getConstraint1DOF("gripper_1_joint_base_x").getMotor1D()
motor_y_1 = sim.getConstraint1DOF("gripper_1_joint_base_y").getMotor1D()
key_motor_map = {agxSDK.GuiEventListener.KEY_Up: (motor_y_0, 0.5),
agxSDK.GuiEventListener.KEY_Down: (motor_y_0, -0.5),
agxSDK.GuiEventListener.KEY_Right: (motor_x_0, 0.5),
agxSDK.GuiEventListener.KEY_Left: (motor_x_0, -0.5),
120: (motor_x_1, 0.5),
60: (motor_x_1, -0.5),
97: (motor_y_1, 0.5),
121: (motor_y_1, -0.5)}
sim.add(KeyboardMotorHandler(key_motor_map))
rbs = dlo.getRigidBodies()
for i in range(len(rbs)):
rbs[i].setName('dlo_' + str(i+1))
return sim
def 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
