def init(self, width, length, rFender):
ship = agx.RigidBody()
self.add(ship)
self.m_body = ship
self.m_body.setName('boat')
half_length = length * 0.5
half_width = width * 0.5
half_height = 0.25 * half_width
b = agxCollide.Geometry(agxCollide.Box(half_length, half_width, half_height))
b.setName('ship')
"""Capsules"""
radius = half_height * 1.2
left_c = agxCollide.Geometry(agxCollide.Capsule(radius, length - 2 * radius))
left_c.setRotation(agx.Quat(math.pi * 0.5, agx.Vec3.Z_AXIS()))
left_c.setPosition(0, half_width - radius, - (half_height + radius))
right_capsules = agxCollide.Geometry(agxCollide.Capsule(radius, length - 2 * radius))
right_capsules.setRotation(agx.Quat(math.pi * 0.5, agx.Vec3.Z_AXIS()))
right_capsules.setPosition(0, radius - half_width, - (half_height + radius))
"""Fender"""
fender_material = agx.Material("fenderMaterial")
landing_material = agx.Material("landingMaterial")
contact_material = demoutils.sim().getMaterialManager().getOrCreateContactMaterial(fender_material,
landing_material)
contact_material.setYoungsModulus(5e5)
contact_material.setFrictionCoefficient(1.0)
contact_material.setDamping(0.4)
self.create_fenders(fender_material, rFender, half_width, half_height, half_length)
"""Top"""
t_box = agxCollide.Geometry(agxCollide.Box(half_length * 0.5, half_width * 0.5, half_height))
t_box.setPosition(-0.4, 0, 2 * half_height)
tt_box = agxCollide.Geometry(agxCollide.Box(half_length * 0.2, half_width * 0.4, half_height * 1.1))
tt_box.setPosition(0, 0, 4.1 * half_height)
"""Assemble ship"""
ship.add(b) # base
ship.add(left_c) # left capsule
ship.add(right_capsules) # left fender
ship.add(t_box) # box on top of base
ship.add(tt_box) # box on top of box on top of base
ship.setPosition(-90, 0, 0)
self.n = 1
self.m_left_propeller = agx.Vec3(-half_length, half_width - radius, - (half_height + 2 * radius))
self.m_right_propeller = agx.Vec3(-half_length, radius - half_width, - (half_height + 2 * radius))
def create_wire(density, size, body_1, pos_1, body_2, pos_2):
"""
Args:
density:
size:
body_1:
pos_1:
body_2:
pos_2:
Returns:
"""
wire_material = agx.Material('wireMaterial')
wire_material.getBulkMaterial().setDensity(density)
wire = agxWire.Wire(size, WIRE_RESOLUTION, False)
wire.setLinearVelocityDamping(0.8)
wire.setMaterial(wire_material)
wire.add(agxWire.BodyFixedNode(body_1.m_body, pos_1))
wire.add(agxWire.BodyFixedNode(body_2.link1, pos_2))
wire.setEnableCollisions(True)
wire.setEnableCollisions(body_1.m_body, False)
wire.setEnableCollisions(body_2.link1, False)
wire_renderer = agxOSG.WireRenderer(wire, demoutils.root())
return wire, wire_renderer
def __init__(self, num_servos: int = 2):
super().__init__()
self.servos = [] # type: list[agx.Hinge]
self.sensors = [] # type: list[agx.RigidBody]
self.material = agx.Material("snake_material_{}".format(self.getUuid().__str__))
self.len = 0
def connect(rb1: agx.RigidBody, rb2: agx.RigidBody):
axis = agx.Vec3(0, 0, -1)
pos = agx.Vec3(0.0, 0.007, 0)
hinge = snakeapp.create_constraint(
pos=pos, axis=axis, c=agx.Hinge, rb1=rb1, rb2=rb2) # type: agx.Hinge
hinge.setCompliance(1E-12)
hinge.getMotor1D().setEnable(True)
hinge.getMotor1D().setCompliance(1E-12)
hinge.getLock1D().setEnable(False)
hinge.getRange1D().setEnable(True)
hinge.getRange1D().setRange(-math.pi / 2, math.pi)
self.add(hinge)
self.servos.append(hinge)
def add(part):
self.len += part.len
self.add(part)
last_part = None
for i in range(0, num_servos):
if i == 0:
type_a = TypeA(self.material)
type_b = TypeB(self.material)
type_b.setPosition(type_a.len, 0, 0)
add(type_a)
add(type_b)
connect(type_a.bottom, type_b.upper)
self.sensors.append(type_a.intermediate.sensor)
self.sensors.append(type_b.intermediate.sensor)
last_part = type_b
elif i == num_servos - 1:
type_b = last_part # type: TypeB
type_c = TypeC(self.material)
type_c.setPosition(type_b.getPosition().x() + type_b.len, 0, 0)
add(type_c)
connect(type_b.bottom, type_c.upper)
self.sensors.append(type_c.intermediate.sensor)
last_part = type_c
else:
type_b1 = last_part # type: TypeB
type_b2 = TypeB(self.material)
type_b2.setPosition(type_b1.getPosition().x() + type_b1.len, 0, 0)
add(type_b2)
connect(type_b1.bottom, type_b2.upper)
self.sensors.append(type_b2.intermediate.sensor)
last_part = type_b2
self.num_servos = len(self.servos)
self.num_sensors = len(self.sensors)
def make_water(water_density, water_length, water_width, water_height):
"""
Args:
water_density:
water_length:
water_width:
water_height:
Returns:
"""
seafloorImageBuilder.save_new_seafloor_image(width=water_length)
water_material = agx.Material("waterMaterial")
water_material.getBulkMaterial().setDensity(water_density)
water_geometry = agxCollide.Geometry(
agxCollide.Box(water_length, water_width / 2, water_height / 2))
water_geometry.setPosition(0, 0, -water_height / 2)
seafloor = MakeWater.make_seafloor(vairance=SEAFLOOR_VARIANCE,
length=water_length * 2,
width=water_width,
depth=-water_height)
"""Surface of water at z = 0."""
water_geometry.setMaterial(water_material)
MakeWater.add_color(water_geometry)
""" ads visualisation"""
agxOSG.createVisual(seafloor, demoutils.root())
print("build water and seafloor")
return water_geometry, seafloor
def __init__(self, num_modules: int, pitch_only=False):
super().__init__()
self.material = agx.Material("snake_material_{}".format(
self.getUuid().__str__))
self.modules = []
for i in range(0, num_modules):
module = SnakeModule(self.material)
module.setPosition(module_len * i, 0, 0)
module.setRotation(agx.EulerAngles(math.pi / 2, 0, 0))
if not pitch_only:
if i % 2 != 0:
module.setRotation(agx.EulerAngles(0, 0, 0))
if i > 0:
merged_body = agx.MergedBody()
merged_body.add(
agx.MergedBodyEmptyEdgeInteraction(
self.modules[i - 1].upper, module.bottom))
snakeapp.add(merged_body)
self.modules.append(module)
self.add(module)
def create_slope() -> agx.RigidBody:
slope_body = agx.RigidBody(agxCollide.Geometry(agxCollide.Box(1, 1, 0.05)))
snakeapp.create_visual(slope_body, diffuse_color=Color.Red())
# slope_body.setPosition(agx.Vec3(0, 0, 0))
slope_body.setRotation(agx.EulerAngles(0, -math.radians(slope_angle),
0)) # Rotate 30 deg around Y.
slope_body.setMotionControl(agx.RigidBody.STATIC)
material = agx.Material("slopeMaterial")
slope_body.getGeometries()[0].setMaterial(material)
return slope_body
def setup_material_coff(sim):
""" Set up all the materials properties for the simulation """
brick_damping = 1.0/4
brick_youngs = 2.0E9
brick_material = agx.Material("brick_material")
brick_material.getSurfaceMaterial().setRoughness(0.5)
brick_material.getSurfaceMaterial().setViscosity(1.e-15)
brick_bulk = brick_material.getBulkMaterial()
brick_bulk.setDamping(brick_damping)
brick_bulk.setYoungsModulus(brick_youngs)
gripper_material = agx.Material("gripper_material")
gripper_material.getSurfaceMaterial().setRoughness(50)
gripper_bulk = gripper_material.getBulkMaterial()
gripper_bulk.setDamping(brick_damping)
gripper_bulk.setYoungsModulus(brick_youngs)
table_material = agx.Material("table_material")
table_material.getSurfaceMaterial().setRoughness(10)
table_bulk = table_material.getBulkMaterial()
table_bulk.setDamping(brick_damping * 100)
table_bulk.setYoungsModulus(brick_youngs * 100)
friction_model = agx.IterativeProjectedConeFriction()
friction_model.setSolveType(agx.FrictionModel.SPLIT)
cm_brick_2_brick = sim.getMaterialManager().getOrCreateContactMaterial(brick_material, brick_material)
cm_brick_2_brick.setFrictionModel(friction_model)
cm_brick_2_brick.setRestitution(0.5)
cm_brick_2_table = sim.getMaterialManager().getOrCreateContactMaterial(brick_material, table_material)
cm_brick_2_table.setFrictionModel(friction_model)
cm_brick_2_table.setRestitution(0.5)
cm_gripper_2_brick = sim.getMaterialManager().getOrCreateContactMaterial(gripper_material, brick_material)
friction_model_gripper = agx.IterativeProjectedConeFriction()
friction_model_gripper.setSolveType(agx.FrictionModel.DIRECT) #Gripper may slip with inaccurate SPLIT solver
cm_gripper_2_brick.setFrictionModel(friction_model_gripper)
cm_gripper_2_brick.setRestitution(0.1)
return table_material, gripper_material, brick_material
def createPond(sim, root):
water_material = agx.Material("waterMaterial")
water_material.getBulkMaterial().setDensity(1025)
water = agxCollide.Geometry(agxCollide.Box(30, 50, 5))
water.setMaterial(water_material)
water.setPosition(agxVec([10,-8,45]))
sim.add(water)
controller = agxModel.WindAndWaterController()
controller.addWater(water)
create_water_visual(water, root)
sim.add(controller)
def __init__(self, length: float = 0.5, angle: float = 20):
super().__init__()
self._slope_angle = math.radians(angle)
self.material = agx.Material("obstacle_material_{}".format(self.getUuid().__str__))
def add_floor():
h = 0.1
floor = agxCollide.Geometry(agxCollide.Box(2.5, 0.5, h), agx.AffineMatrix4x4.translate(0, 0, -h))
snakeapp.create_visual(floor, diffuse_color=agxRender.Color.Green())
self.add(floor)
add_floor()
def add_slope():
half_extents = agx.Vec3(length, 0.1, 0.005)
slope = agxCollide.Geometry(agxCollide.Box(half_extents),
agx.AffineMatrix4x4.translate(half_extents.x(), 0, -half_extents.z()))
slope.setRotation(agx.EulerAngles(0, -self._slope_angle, 0))
snakeapp.create_visual(slope, diffuse_color=agxRender.Color.Red())
slope.setMaterial(self.material)
self.add(slope)
return slope
self.slope = add_slope()
def add_box():
height = math.sin(self._slope_angle) * length
half_extents = agx.Vec3(0.5, 0.1, height)
box = agxCollide.Geometry(agxCollide.Box(half_extents),
agx.AffineMatrix4x4.translate(0, 0, half_extents.z()))
box.setParentFrame(self.slope.getParentFrame())
box.setLocalPosition(0.5 + math.cos(self._slope_angle), 0, 0)
snakeapp.create_visual(box, diffuse_color=agxRender.Color.Yellow())
box.setMaterial(self.material)
self.add(box)
add_box()
def addGround(MiroSystem, size_x, size_y, size_z, pos, heightmap, texture='test.jpg', scale=[4,3], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, mass=False, density=1000, dynamic=False, color=[0.5, 0.5, 0.5]):
agxSim = agxPython.getContext().environment.getSimulation()
agxApp = agxPython.getContext().environment.getApplication()
agxRoot = agxPython.getContext().environment.getSceneRoot()
# Create the ground
ground_material = agx.Material("Ground")
# Create the height field from a heightmap
hf = agxCollide.HeightField.createFromFile("textures/"+heightmap, size_x, size_z, 0, size_y)
ground_geometry = agxCollide.Geometry(hf)
ground = agx.RigidBody(ground_geometry)
ground.setPosition(agxVecify(pos))
ground.setMotionControl(agx.RigidBody.STATIC)
node = agxOSG.createVisual( ground, agxRoot )
agxOSG.setShininess(node, 5)
# Add a visual texture.
agxOSG.setTexture(node, "textures/"+texture, True, agxOSG.DIFFUSE_TEXTURE, 100, 100)
agxSim.add(ground)
def __init__(self):
hf = agxCollide.HeightField.createFromFile("assets/textures/dirtRoad128.png", 6, 6, -0.5, 0.5)
terrain = agxModel.Terrain(hf)
snakeapp.add(terrain)
self.material = agx.Material("GroundMaterial")
terrain_data = terrain.getDataInterface() # type: agxModel.TerrainDataInterface
terrain_data.setMaxCompressionMultiplier(32)
particle_adhesion = 0
deformity = 100
angle_of_repose = math.pi * 0.2
material_index = 0
terrain_data.add(self.material, material_index, particle_adhesion, deformity, angle_of_repose)
range_for_youngs_modulus = agx.RangeReal(10000, 20000)
terrain_renderer = agxOSG.TerrainRenderer(terrain, range_for_youngs_modulus, snakeapp.root())
snakeapp.add(terrain_renderer)
terrain_visual = terrain_renderer.getTerrainNode()
agxOSG.setDiffuseColor(terrain_visual, Color(0.7, 0.7, 0.8, 1))
agxOSG.setSpecularColor(terrain_visual, Color(0.4, 0.4, 0.4, 1))
agxOSG.setShininess(terrain_visual, 128)
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, 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
def build_material(name, density):
material = agx.Material(name)
material.getBulkMaterial().setDensity(density)
return material
def __init__(self, num_robots: int = 1):
super().__init__()
self.jointCon = [] # type: list[agx.Hinge]
self.forceFeedback = [] # type: list[agx.RigidBody]
self.len = 0
self.material = agx.Material("robot_material{}".format(
self.getUuid().__str__))
def connect(rb1: agx.RigidBody, rb2: agx.RigidBody):
axis = agx.Vec3(0, 0, 0)
pos = agx.Vec3(
0,
0,
0,
)
hinge = oneLegRobotApp.create_constraint(
pos=pos, axis=axis, c=agx.Hinge, rb1=rb1,
rb2=rb2) # type: agx.Hinge
hinge.setCompliance(1E-12)
hinge.getMotor1D().setEnable(True)
hinge.getMotor1D().setCompliance(1E-12)
hinge.getLock1D().setEnable(False)
hinge.getRange1D().setEnable(True)
hinge.getRange1D().setRange(-math.pi / 2, math.pi)
self.add(hinge)
self.jointCon.append(hinge)
# def add(part):
# self.len += part.len
# self.add(part)
#
# last_part = None
#testRobotLeg = OneLegRobotAssembly(self.material)
self.floor = OneLegRobotAssembly(self.material).create_floor()
self.first_joint_geometry_aft = OneLegRobotAssembly(
self.material).create_section(20, -180 / 2, 440, 0, 0, math.pi / 2,
first_joint_shape_aft)
self.second_joint_geometry_aft = OneLegRobotAssembly(
self.material).create_section(0, -180 / 2, 320, 0, 0, math.pi / 2,
second_joint_shape_aft)
self.first_joint_geometry_fwd = OneLegRobotAssembly(
self.material).create_section(0, 180 / 2, 440, 0, 0, math.pi / 2,
first_joint_shape_fwd)
self.second_joint_geometry_fwd = OneLegRobotAssembly(
self.material).create_section(-20, 180 / 2, 320, 0, 0, math.pi / 2,
second_joint_shape_fwd)
# Connect joints
connect(self.first_joint_geometry_aft, self.second_joint_geometry_aft)
connect(self.first_joint_geometry_fwd, self.second_joint_geometry_fwd)
# Build the robot
oneLegRobotApp.add(self.floor)
self.second_joint_geometry_aft.setParentFrame(
self.first_joint_geometry_aft.getFrame())
self.second_joint_geometry_fwd.setParentFrame(
self.first_joint_geometry_fwd.getFrame())
oneLegRobotApp.add(self.first_joint_geometry_aft)
oneLegRobotApp.add(self.second_joint_geometry_aft)
oneLegRobotApp.add(self.first_joint_geometry_fwd)
oneLegRobotApp.add(self.second_joint_geometry_fwd)
# Arrange camera to be centered on floor
oneLegRobotApp.init_camera(eye=agx.Vec3(800, 0, 800),
center=self.floor.getPosition())
def __init__(self, **kwargs):
"""
Construct given
Arguments:
local_transform: agx.AffineMatrix4x4 - [Optional] local transform so that track y axis is the rotation axis.
"""
super().__init__()
self.m_sprocket_hinge = kwargs[ 'sprocket_hinge' ]
track_material = kwargs.get('track_material', agx.Material("DefaultTrackMaterial"))
sprocket_body = kwargs[ 'sprocket_body' ]
self.add(sprocket_body)
sprocket_body_info = RigidBodyInfo( sprocket_body )
# We will assume all sprockets/Rollers/Idlers has the same material!
self.m_roller_material = None
# Get the material from the first geometry which has collision enabled
for g in sprocket_body_info.geometries:
if (g.collisions_enabled):
self.m_roller_material = g.instance.getMaterial()
break
assert(self.m_roller_material)
idler_body = kwargs['idler_body']
self.add(idler_body)
roller_bodies = kwargs['roller_bodies']
local_transform = kwargs.get( 'local_transform', agx.AffineMatrix4x4() )
track_settings = kwargs.get( 'track_settings')
self.m_track = agxVehicle.Track( track_settings.track_parameters.number_of_shoes,
track_settings.track_parameters.width,
track_settings.track_parameters.thickness,
track_settings.track_parameters.initial_offset )
self.m_track.setMaterial(track_material)
radius = Track.find_radius( sprocket_body )
sprocket = agxVehicle.TrackWheel( agxVehicle.TrackWheel.SPROCKET,
radius, sprocket_body, local_transform )
self.m_track.add( sprocket )
radius = Track.find_radius( idler_body )
idler = agxVehicle.TrackWheel( agxVehicle.TrackWheel.IDLER,
radius, idler_body, local_transform )
self.m_track.add( idler )
idler.getLocalFrame().setLocalMatrix( agx.AffineMatrix4x4.rotate( agx.Vec3.Y_AXIS(),
agx.Vec3.X_AXIS() ) )
for b in roller_bodies:
self.add(b)
radius = Track.find_radius( b )
roller = agxVehicle.TrackWheel( agxVehicle.TrackWheel.ROLLER,
radius, b, local_transform )
roller.setEnableProperty( agxVehicle.TrackWheel.SPLIT_SEGMENTS, True )
self.m_track.add( roller )
track_merge_parameters_settings = track_settings.track_parameters.internal_merge
mergeProperties = self.m_track.getInternalMergeProperties()
mergeProperties.setEnableMerge( track_merge_parameters_settings.enabled)
mergeProperties.setNumNodesPerMergeSegment( track_merge_parameters_settings.number_of_nodes_per_segment )
mergeProperties.setEnableLockToReachMergeCondition( track_merge_parameters_settings.use_lock )
mergeProperties.setLockToReachMergeConditionCompliance( track_merge_parameters_settings.lock_compliance )
mergeProperties.setLockToReachMergeConditionDamping( track_merge_parameters_settings.lock_damping )
mergeProperties.setMaxAngleMergeCondition( track_merge_parameters_settings.merge_angle )
mergeProperties.setContactReduction( track_merge_parameters_settings.contact_reduction_level )
track_properties_settings = track_settings.track_parameters.properties
properties = self.m_track.getProperties()
properties.setHingeCompliance( track_properties_settings.compliance )
properties.setHingeDamping( track_properties_settings.damping )
properties.setEnableHingeRange( track_properties_settings.range_enable )
properties.setHingeRangeRange( track_properties_settings.range_range.lower,
track_properties_settings.range_range.upper)
properties.setEnableOnInitializeMergeNodesToWheels( track_properties_settings.on_initialize_merge_nodes_to_wheels )
properties.setEnableOnInitializeTransformNodesToWheels( track_properties_settings.on_initialize_transform_nodes_to_wheels )
properties.setTransformNodesToWheelsOverlap( track_properties_settings.target_node_wheel_overlap )
properties.setNodesToWheelsMergeThreshold( track_properties_settings.node_wheel_merge_threshold )
properties.setNodesToWheelsSplitThreshold( track_properties_settings.node_wheel_split_threshold)
properties.setNumNodesIncludedInAverageDirection( track_properties_settings.num_nodes_in_averge_direction)
properties.setMinStabilizingHingeNormalForce( track_properties_settings.min_stabilizing_normal_force)
properties.setStabilizingHingeFrictionParameter( track_properties_settings.stabilizing_friction_parameter)
self.add(self.m_track)
self.m_track.initialize()
print( "Loading shoe visual: ", track_settings.shoe_visual_filename)
shoe_visual_data = agxOSG.readNodeFile( track_settings.shoe_visual_filename, False )
assert( shoe_visual_data )
print( " ...done." )
shoe_visual_transform = agxOSG.Transform()
shoe_visual_transform.addChild( shoe_visual_data )
nodes = self.m_track.nodes()
it = nodes.begin()
while it != nodes.end():
node = it.get()
gn = agxOSG.GeometryNode( node.getRigidBody().getGeometries()[ 0 ] )
gn.addChild( shoe_visual_transform )
root().addChild( gn )
it.inc()
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 _initialize(self, data: SimulationFile, **kwargs):
self.m_body_chassie = data.bodies[ 'ChassieBody' ]
self.m_body_under_carriage = data.bodies[ 'UnderCarriageBody' ]
self.m_body_bucket = data.bodies[ 'Bucket' ]
self.m_body_sprockets = [data.bodies['LeftFrontSprocket'], data.bodies['RightFrontSprocket']]
self.m_body_idlers = [data.bodies['LeftRearIdler'], data.bodies['RightRearIdler']]
self.m_sprocket_hinges = [data.constraints['LeftSprocketHinge'].asHinge(), data.constraints['RightSprocketHinge'].asHinge()]
self.m_left_roller_bodies = []
self.m_right_roller_bodies = []
for i in range(1,10):
name = 'RightRoller'+str(i)
if name in data.bodies:
self.m_right_roller_bodies.append(data.bodies[name])
name = 'LeftRoller'+str(i)
if name in data.bodies:
self.m_left_roller_bodies.append(data.bodies[name])
self.m_under_carriage_observer = data.observers['UnderCarriageObserver']
# Creating tracks: [left,right]
# The local transform makes sure the y axes are pointing out from
# the track center positions.
local_track_transform = agx.AffineMatrix4x4.rotate( agx.Vec3.Z_AXIS(), agx.Vec3.Y_AXIS() )
track_settings = kwargs.get( 'track_settings', self.default_track_settings() )
track_material = agx.Material("TrackMaterial")
self.m_tracks = [
Track( track_settings = track_settings,
sprocket_hinge = self.sprocket_hinge(self.Location.LEFT),
sprocket_body = self.sprocket_body(self.Location.LEFT),
idler_body = self.idler_body(self.Location.LEFT),
roller_bodies = self.roller_bodies(self.Location.LEFT),
local_transform = local_track_transform,
track_material = track_material ),
Track( track_settings = track_settings,
sprocket_hinge = self.sprocket_hinge(self.Location.RIGHT),
sprocket_body = self.sprocket_body(self.Location.RIGHT),
idler_body = self.idler_body(self.Location.RIGHT),
roller_bodies = self.roller_bodies(self.Location.RIGHT),
local_transform = local_track_transform,
track_material = track_material )
]
for t in self.m_tracks:
self.add(t)
t.track_material = track_material
self.configure_track_roller_contact_materials(self.m_tracks[0].track_material, self.m_tracks[0].roller_material)
self.initialize_bucket_data(data)
###################################################################################
# Drivetrain #
###################################################################################
###################################################################################
driveline_settings = kwargs.get( 'driveline_settings', self.default_driveline_settings() )
def setup_sprocket_hinge(hinge, driveline_settings):
hinge.getMotor1D().setForceRange( agx.RangeReal(driveline_settings.max_torque))
hinge.getMotor1D().setCompliance( driveline_settings.compliance )
hinge.getMotor1D().setSpeed( driveline_settings.speed )
hinge.getMotor1D().setLockedAtZeroSpeed( driveline_settings.lockedAtZeroSpeed )
setup_sprocket_hinge(self.sprocket_hinge(self.Location.LEFT), driveline_settings.hinge)
setup_sprocket_hinge(self.sprocket_hinge(self.Location.RIGHT), driveline_settings.hinge)
# Get the constraints for controlling the cabin/arm/boom
self.m_boom_prismatics = [data.constraints['BoomPrismatic1'].asPrismatic(), data.constraints['BoomPrismatic2'].asPrismatic()]
self.m_bucket_prismatic = data.constraints['BucketPrismatic'].asPrismatic()
self.m_arm_prismatic = data.constraints['ArmPrismatic'].asPrismatic()
self.m_cabin_hinge = data.constraints['CabinHinge'].asHinge()
self.m_keyboard_controls = None
self.m_gamepad_controls = None
self.keyboard_controls = kwargs.get( 'keyboard_controls', None )
self.gamepad_controls = kwargs.get( 'gamepad_controls', None )
def add_boxShape(MiroSystem, size_x, size_y, size_z, pos, texture=False, scale=[4,3], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, mass=False, density=1000, dynamic=False, color=[0.5, 0.5, 0.5], friction=False):
'''system, size_x, size_y, size_z, pos, texture, scale = [5,5], hitbox = True/False'''
# Convert position to chrono vector, supports using chvector as input as well
agxSim = agxPython.getContext().environment.getSimulation()
agxApp = agxPython.getContext().environment.getApplication()
agxRoot = agxPython.getContext().environment.getSceneRoot()
agxPos = agxVecify(pos)
agxRotAxis = agxVecify(rotAxis)
[size_x, size_y, size_z] = xyzTransform([size_x, size_y, size_z], True)
scale = [scale[0]/4, scale[1]/3]
# Create a box
body_geo = agxCollide.Geometry( agxCollide.Box(size_x/2, size_y/2, size_z/2))
body_geo.setName("body")
if friction:
high_friction_tires = agx.Material('Tires', 0.05, friction)
body_geo.setMaterial(high_friction_tires)
body_geo.setEnableCollisions(Collide)
body_box = agx.RigidBody(body_geo)
if mass:
body_box.getMassProperties().setMass(mass)
else:
body_box.getMassProperties().setMass(body_geo.calculateVolume()*density)
if Fixed:
body_box.setMotionControl(1)
body_box.setPosition(agxPos)
rotateBody(body_box, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees)
# Collision shape
# if(Collide):
# Visualization shape
body_shape = agxOSG.createVisual(body_box, agxRoot)
# Body texture
if texture:
# Filter 'textures/' out of the texture name, it's added later
if len(texture) > len('textures/'):
if texture[0:len('textures/')] == 'textures/':
texture = texture[len('textures/'):]
if TEXTURES_ON or texture in important_textures:
if texture not in LOADED_TEXTURES.keys():
agxTex = agxOSG.createTexture(TEXTURE_PATH+texture)
LOADED_TEXTURES.update({texture: agxTex})
if TEXTURE_PATH == 'textures_lowres/' and texture=='yellow_brick.jpg':
scale[0] = 11*scale[0]
scale[1] = 8*scale[1]
agxOSG.setTexture(body_shape, TEXTURE_PATH+texture, True, agxOSG.DIFFUSE_TEXTURE, scale[0], scale[1])
else:
color = backupColor(texture, color)
texture = False
if not texture:
agxColor = agxRender.Color(color[0], color[1], color[2])
agxOSG.setDiffuseColor(body_shape, agxColor)
if len(color) > 3:
agxOSG.setAlpha(body_shape, color[3])
agxSim.add(body_box)
return body_box
def create_rigid_body_material_from_particle_material(pmat):
mat = agx.Material("rb_%s" % pmat.getName())
mat.getBulkMaterial().setDensity(pmat.getBulkMaterial().getDensity())
# mat.getBulkMaterial().setYoungsModulus(pmat.getBulkMaterial().getYoungsModulus())
# mat.getBulkMaterial().setPoissonsRatio(pmat.getBulkMaterial().getPoissonsRatio())
return mat
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 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 buildScene1(self, app, sim, root):
# Create the Terrain
num_cells_x = 80
num_cells_y = 80
cell_size = 0.15
max_depth = 1.0
agx_heightField = agxCollide.HeightField(num_cells_x, num_cells_y,
(num_cells_x - 1) * cell_size,
(num_cells_y - 1) * cell_size)
# Define the initial height field (random or fixed) depending on if if data collection or test
if self.control_mode == "data_collection":
np_heightField = self.createRandomHeightfield(
num_cells_x, num_cells_y, cell_size)
elif self.control_mode == "mpcc" or self.control_mode == "trajectory_control":
np_heightField = self.createRandomHeightfield(
num_cells_x, num_cells_y, cell_size)
if self.set_height_from_previous:
agx_heightField = self.agx_heightField_previous
else:
agx_heightField = self.setHeightField(agx_heightField,
np_heightField)
terrain = agxTerrain.Terrain.createFromHeightField(
agx_heightField, 5.0)
sim.add(terrain)
# Define Gravity
G = agx.Vec3(0, 0, -10.0)
sim.setUniformGravity(G)
# define the material
terrain.loadLibraryMaterial("sand_1")
terrainMaterial = terrain.getTerrainMaterial()
terrainMaterial.getBulkProperties().setSwellFactor(1.00)
compactionProperties = terrainMaterial.getCompactionProperties()
compactionProperties.setAngleOfReposeCompactionRate(500.0)
terrain.setCompaction(1.05)
# The trenching will reach the bounds of the terrain so we simply remove particles out of bounds
# to get rid of the mateiral in a practical way.
terrain.getProperties().setDeleteSoilParticlesOutsideBounds(True)
if app:
# Setup a renderer for the terrain
renderer = agxOSG.TerrainVoxelRenderer(terrain, root)
renderer.setRenderHeightField(True)
# We choose to render the compaction of the soil to visually denote excavated
# soil from compacted ground
# renderer.setRenderCompaction( True, agx.RangeReal( 1.0, 1.05 ) )
renderer.setRenderHeights(True, agx.RangeReal(-0.4, 0.1))
# renderer.setRenderHeights(True, agx.RangeReal(-0.5,0.5))
renderer.setRenderVoxelSolidMass(False)
renderer.setRenderVoxelFluidMass(False)
renderer.setRenderNodes(False)
renderer.setRenderVoxelBoundingBox(False)
renderer.setRenderSoilParticlesMesh(True)
sim.add(renderer)
# Set contact materials of the terrain and shovel
# This contact material governs the resistance that the shovel will feel when digging into the terrain
# [ Shovel - Terrain ] contact material
shovelMaterial = agx.Material("shovel_material")
terrainMaterial = terrain.getMaterial(
agxTerrain.Terrain.MaterialType_TERRAIN)
shovelTerrainContactMaterial = agx.ContactMaterial(
shovelMaterial, terrainMaterial)
shovelTerrainContactMaterial.setYoungsModulus(1e8)
shovelTerrainContactMaterial.setRestitution(0.0)
shovelTerrainContactMaterial.setFrictionCoefficient(0.4)
sim.add(shovelTerrainContactMaterial)
# Create the trenching shovel body creation, do setup in the Terrain object and
# constrain it to a kinematic that will drive the motion
# Create the bucket rigid body
cuttingEdge, topEdge, forwardVector, bucket = self.createBucket(
default)
sim.add(bucket)
# Create the Shovel object using the previously defined cutting and top edge
shovel = agxTerrain.Shovel(bucket, topEdge, cuttingEdge, forwardVector)
agxUtil.setBodyMaterial(bucket, shovelMaterial)
# Set a margin around the bounding box of the shovel where particles are not to be merged
shovel.setNoMergeExtensionDistance(0.1)
# Add the shovel to the terrain
terrain.add(shovel)
if app: # and self.consecutive_scoop_i < 4:
# Create visual representation of the shovel
node = agxOSG.createVisual(bucket, root)
agxOSG.setDiffuseColor(node, agxRender.Color.Gold())
agxOSG.setAlpha(node, 1.0)
# Set initial bucket rotation
if self.control_mode == "mpcc":
angle_bucket_initial = -0.3 * np.pi
else:
angle_bucket_initial = np.random.uniform(low=-0.25 * np.pi,
high=-0.35 * np.pi)
bucket.setRotation(agx.EulerAngles(0.0, angle_bucket_initial, agx.PI))
# Get the offset of the bucket tip from the COM
tip_offset = shovel.getCuttingEdgeWorld().p2
#
inertia_tensor = bucket.getMassProperties().getInertiaTensor()
mass = bucket.getMassProperties().getMass()
h_offset_sqrd = tip_offset[0]**2 + tip_offset[2]**2
inertia_bucket = inertia_tensor.at(1, 1) + mass * h_offset_sqrd
# Set initial bucket position (for consecutive scoops)
if self.control_mode == "mpcc" or self.control_mode == "trajectory_control":
if self.consecutive_scoop_i == 0:
x_initial_tip = -4.0
elif self.consecutive_scoop_i == 1:
x_initial_tip = -3.6
elif self.consecutive_scoop_i == 2:
x_initial_tip = -3.3
else:
x_initial_tip = -2.6
else:
x_initial_tip = np.random.uniform(low=-4.5, high=-3.0)
# find the soil height at the initial penetration location
hf_grid_initial = terrain.getClosestGridPoint(
agx.Vec3(x_initial_tip, 0.0, 0.0))
height_initial = terrain.getHeight(hf_grid_initial) - 0.05
# Set the initial bucket location such that it is just contacting the soil
position = agx.Vec3(x_initial_tip - tip_offset[0], 0,
height_initial - tip_offset[2])
bucket.setPosition(terrain.getTransform().transformPoint(position))
bucket.setVelocity(0.0, 0.0, 0.0)
# bucket.setAngularVelocity(0.0, 0.05, 0.0)
# Add a lockjoint between a kinematic sphere and the shovel
# in order to have some compliance when moving the shovel
# through the terrain
offset = agx.Vec3(0.0, 0.0, 0.0)
## ADD ALL THE JOINTS TO CONTROL THE BUCKET (x,z,theta)
sphere1 = agx.RigidBody(agxCollide.Geometry(agxCollide.Sphere(.1)))
sphere2 = agx.RigidBody(agxCollide.Geometry(agxCollide.Sphere(.1)))
sphere3 = agx.RigidBody(agxCollide.Geometry(agxCollide.Sphere(.1)))
sphere1.setMotionControl(agx.RigidBody.DYNAMICS)
sphere2.setMotionControl(agx.RigidBody.DYNAMICS)
sphere3.setMotionControl(agx.RigidBody.DYNAMICS)
sphere1.getGeometries()[0].setEnableCollisions(False)
sphere2.getGeometries()[0].setEnableCollisions(False)
sphere3.getGeometries()[0].setEnableCollisions(False)
tip_position = shovel.getCuttingEdgeWorld().p2
tip_position[1] = bucket.getCmPosition()[1]
sphere1.setPosition(tip_position)
sphere2.setPosition(tip_position)
sphere3.setPosition(tip_position)
sphere1.getMassProperties().setMass(0.000001)
sphere2.getMassProperties().setMass(0.000001)
sphere3.getMassProperties().setMass(0.000001)
# print('sphere mass: ', sphere1.getMassProperties().getMass())
sim.add(sphere1)
sim.add(sphere2)
sim.add(sphere3)
# Set prismatic joint for x transalation world - sphere 1
f1 = agx.Frame()
f1.setLocalRotate(agx.EulerAngles(0, math.radians(90), 0))
prismatic1 = agx.Prismatic(sphere1, f1)
# Set prismatic joint for z transalation world - sphere 2
f1 = agx.Frame()
f2 = agx.Frame()
f1.setLocalRotate(agx.EulerAngles(0, math.radians(180), 0))
f2.setLocalRotate(agx.EulerAngles(0, math.radians(180), 0))
prismatic2 = agx.Prismatic(sphere1, f1, sphere2, f2)
# # Set hinge joint for rotation of the bucket
f1 = agx.Frame()
f1.setLocalRotate(agx.EulerAngles(-math.radians(90), 0, 0))
f2 = agx.Frame()
f2.setLocalRotate(agx.EulerAngles(-math.radians(90), 0, 0))
hinge2 = agx.Hinge(sphere2, f1, sphere3, f2)
sim.add(prismatic1)
sim.add(prismatic2)
sim.add(hinge2)
lock = agx.LockJoint(sphere3, bucket)
sim.add(lock)
# Uncomment to lock rotations
# sim.add(agx.LockJoint(sphere2,bucket))
# constant force and torque
operations = [agx.Vec3(0.0, 0.0, 0.0), agx.Vec3(0.0, 0.0, 0.0)]
# Extract soil shape along the bucket excavation direction
x_hf, z_hf = self.extractSoilSurface(terrain, sim)
self.soilShapeEvaluator = SoilSurfaceEvaluator(x_hf, z_hf)
setattr(self.dfl, "soilShapeEvaluator", self.soilShapeEvaluator)
if self.control_mode == "data_collection":
# create driver and add it to the simulation
driver = ForceDriverPID(app, sphere3, lock, hinge2, prismatic1,
prismatic2, terrain, shovel, operations,
self.dt_control)
# Add the current surface evaluator to the controller
setattr(driver, "soilShapeEvaluator", self.soilShapeEvaluator)
# Add the controller to the simulation
sim.add(driver)
elif self.control_mode == "trajectory_control":
# create driver and add it to the simulation
# create driver and add it to the simulation
driver = ForceDriverTrajectory(app, sphere3, lock, hinge2,
prismatic1, prismatic2, terrain,
shovel, operations, self.dt_control)
# Add the current surface evaluator to the controller
setattr(driver, "soilShapeEvaluator", self.soilShapeEvaluator)
setattr(driver, "dfl", self.dfl)
x_path = x_initial_tip + np.array(
[0., 0.5, 1.5, 2.0, 2.5, 3.0, 3.5])
y_soil, _, _, _ = self.soilShapeEvaluator.soil_surf_eval(x_path)
y_path = y_soil + np.array(
[-0.07, -0.25, -0.25, -0.25, -0.25, -0.25, -0.02])
spl_path = spline_path(x_path, y_path)
setattr(driver, "path_eval", spl_path.path_eval)
# Add the controller to the simulation
sim.add(driver)
elif self.control_mode == "mpcc":
# create driver and add it to the simulation
driver = ForceDriverDFL(app, sphere3, lock, hinge2, terrain,
shovel, self.dt_control)
################ MPCC CONTROLLER ############################
# Add the current surface evaluator to the controller
setattr(driver, "soilShapeEvaluator", self.soilShapeEvaluator)
setattr(driver, "dfl", self.dfl)
setattr(driver, "scaling", self.scaling)
x_path = x_initial_tip + np.array([
0.,
0.5,
1.5,
2.0,
2.5,
3.0,
])
y_soil, _, _, _ = self.soilShapeEvaluator.soil_surf_eval(x_path)
y_path = y_soil + np.array(
[-0.07, -0.25, -0.25, -0.25, -0.25, -0.02])
# Set the state constraints
if self.observable_type == "dfl":
x_min = np.array([
x_initial_tip - 0.1, -3., 0.5, -0.5, -2.5, -2.5,
-80000 * self.scaling, -80000 * self.scaling, 0.0,
-70000 * self.scaling, -70000 * self.scaling,
-70000 * self.scaling, -70000 * self.scaling
])
x_max = np.array([
2., 5.0, 2.5, 2.5, 2.5, 2.5, 80000 * self.scaling,
80000 * self.scaling, 3000. * self.scaling,
70000 * self.scaling, 70000 * self.scaling,
70000 * self.scaling, 70000 * self.scaling
])
n_dyn = self.dfl.plant.n
elif self.observable_type == "x":
x_min = np.array(
[x_initial_tip - 0.1, -3., 0.5, -0.5, -2.5, -2.5])
x_max = np.array([2., 5.0, 2.5, 2.5, 2.5, 2.5])
n_dyn = self.dfl.plant.n_x
# Set the input constraints
u_min = np.array([
-100. * self.scaling, -70000 * self.scaling,
-70000 * self.scaling
])
u_max = np.array([
75000. * self.scaling, 70000 * self.scaling,
70000 * self.scaling
])
if self.set_height_from_previous:
pass
else:
self.spl_path = spline_path(x_path, y_path)
# instantiate the MPCC object
mpcc = MPCC(np.zeros((n_dyn, n_dyn)),
np.zeros((n_dyn, self.dfl.plant.n_u)),
x_min,
x_max,
u_min,
u_max,
dt=self.dt_data,
N=50)
# set the observation function, path object and linearization function
setattr(mpcc, "path_eval", self.spl_path.path_eval)
setattr(mpcc, "get_soil_surface",
self.soilShapeEvaluator.soil_surf_eval)
if self.model_has_surface_shape:
print('Linearizing with soil shape')
setattr(mpcc, "get_linearized_model",
self.dfl.linearize_soil_dynamics_koop)
else:
setattr(mpcc, "get_linearized_model",
self.dfl.linearize_soil_dynamics_no_surface)
self.mpcc = copy.copy(mpcc)
pos_tip, vel_tip, acl_tip, ang_tip, omega, alpha = measureBucketState(
sphere3, shovel)
x_i = np.array([
pos_tip[0], pos_tip[2], ang_tip, vel_tip[0], vel_tip[2],
omega[1]
])
eta_i = np.array(
[acl_tip[0], acl_tip[2], alpha[1], 0., 0., 0., 0.])
# Choose the initial path arcposition based on a close initial x tip position
theta_array = np.linspace(-10, 0, num=1000)
for i in range(len(theta_array)):
x_path, y_path = self.spl_path.path_eval(theta_array[i], d=0)
if x_path > pos_tip[0]:
path_initial = theta_array[i]
break
# set initial input (since input cost is differential)
x_0_mpcc = np.concatenate(
(self.dfl.g_Koop(x_i, eta_i, _), np.array([path_initial])))
u_minus_mpcc = np.array([0.0, 0.0, 0.0, 0.0])
driver.last_x_opt = x_0_mpcc
# sets up the new mpcc problem _mpcc
mpcc.setup_new_problem(self.Q_mpcc, self.R_mpcc, self.q_theta_mpcc,
x_0_mpcc, u_minus_mpcc)
setattr(driver, "mpcc", mpcc)
#####################################################################################
# Add the controller to the simulation
sim.add(driver)
# Limit core usage to number of physical cores. Assume that HT/SMT is active
# and divide max threads with 2.
agx.setNumThreads(0)
n = int(agx.getNumThreads() / 2 - 1)
agx.setNumThreads(n)
# Setup initial camera view
if app:
createHelpText(sim, app)
return terrain, shovel, driver, sphere3
def build_simulation(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(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