def _add_rendering(self, mode='osg'):
# Set renderer
self.app.setAutoStepping(True)
self.app.setEnableDebugRenderer(False)
self.app.setEnableOSGRenderer(True)
file_directory = os.path.dirname(os.path.abspath(__file__))
package_directory = os.path.split(file_directory)[0]
gripper_texture = os.path.join(package_directory, TEXTURE_GRIPPER_FILE)
# Create scene graph for rendering
root = self.app.getSceneRoot()
rbs = self.sim.getRigidBodies()
for rb in rbs:
node = agxOSG.createVisual(rb, root)
if rb.getName() == "hollow_cylinder":
agxOSG.setDiffuseColor(node, agxRender.Color_SteelBlue())
agxOSG.setShininess(node, 15)
elif rb.getName() == "gripper_body":
agxOSG.setDiffuseColor(node,
agxRender.Color(1.0, 1.0, 1.0, 1.0))
agxOSG.setTexture(node, gripper_texture, False,
agxOSG.DIFFUSE_TEXTURE)
agxOSG.setShininess(node, 2)
elif "dlo" in rb.getName(): # Cable segments
agxOSG.setDiffuseColor(node,
agxRender.Color(0.0, 1.0, 0.0, 1.0))
else:
agxOSG.setDiffuseColor(node, agxRender.Color.Beige())
agxOSG.setAlpha(node, 0.0)
# Set rendering options
scene_decorator = self.app.getSceneDecorator()
scene_decorator.setEnableLogo(False)
scene_decorator.setBackgroundColor(agxRender.Color(1.0, 1.0, 1.0, 1.0))
def _add_rendering(self):
self.app.setAutoStepping(False)
if not self.root:
self.root = self.app.getRoot()
rbs = self.sim.getRigidBodies()
for rb in rbs:
name = rb.getName()
node = agxOSG.createVisual(rb, self.root)
if name == "ground":
agxOSG.setDiffuseColor(node, agxRender.Color.SlateGray())
elif "gripper_left" in name and "base" not in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Red())
elif "gripper_right" in name and "base" not in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Blue())
elif "pusher" in name and "base" not in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Yellow())
elif "dlo" in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Green())
elif "obstacle" in name or "cylinder" in name:
agxOSG.setDiffuseColor(node, agxRender.Color.SteelBlue())
elif "bounding_box" in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Burlywood())
else:
agxOSG.setAlpha(node, 0)
logger.info("No color set for {}.".format(name))
if "goal" in name and "base" not in name:
agxOSG.setAlpha(node, 0.2)
scene_decorator = self.app.getSceneDecorator()
light_source_0 = scene_decorator.getLightSource(agxOSG.SceneDecorator.LIGHT0)
light_source_0.setPosition(self.light_pose['light_position'])
light_source_0.setDirection(self.light_pose['light_direction'])
scene_decorator.setEnableLogo(False)
scene_decorator.setBackgroundColor(agxRender.Color(1.0, 1.0, 1.0, 1.0))
def _add_rendering(self, mode='osg'):
# Set renderer
self.app.setAutoStepping(True)
self.app.setEnableDebugRenderer(False)
self.app.setEnableOSGRenderer(True)
# Create scene graph for rendering
root = self.app.getSceneRoot()
rbs = self.sim.getRigidBodies()
for rb in rbs:
node = agxOSG.createVisual(rb, root)
if rb.getName() == "ground":
agxOSG.setDiffuseColor(node, agxRender.Color.SlateGray())
elif rb.getName() == "cylinder_top_0" or rb.getName(
) == "cylinder_top_1" or rb.getName() == "cylinder_top_2":
agxOSG.setDiffuseColor(node, agxRender.Color.DarkGray())
elif rb.getName() == "cylinder_inner_0" or rb.getName(
) == "cylinder_inner_1" or rb.getName() == "cylinder_inner_2":
agxOSG.setDiffuseColor(node, agxRender.Color.LightSteelBlue())
elif rb.getName() == "cylinder_low_0" or rb.getName(
) == "cylinder_low_1" or rb.getName() == "cylinder_low_2":
agxOSG.setDiffuseColor(node, agxRender.Color.DarkGray())
elif rb.getName() == "gripper":
agxOSG.setDiffuseColor(node, agxRender.Color.DarkBlue())
elif "dlo" in rb.getName(): # Cable segments
agxOSG.setDiffuseColor(node,
agxRender.Color(0.8, 0.2, 0.2, 1.0))
else:
agxOSG.setDiffuseColor(node, agxRender.Color.Beige())
agxOSG.setAlpha(node, 0.0)
# Set rendering options
scene_decorator = self.app.getSceneDecorator()
scene_decorator.setEnableLogo(False)
scene_decorator.setBackgroundColor(agxRender.Color(1.0, 1.0, 1.0, 1.0))
def add_color(geomerty):
waterNode = agxOSG.createVisual(geomerty, demoutils.root())
color = agxRender.Color.DeepSkyBlue()
alpha = 0.4
agxOSG.setDiffuseColor(waterNode, color)
agxOSG.setAmbientColor(waterNode, agx.Vec4f(1))
agxOSG.setShininess(waterNode, 120)
agxOSG.setAlpha(waterNode, alpha)
def setTriggered(self, trigger_status):
self.triggered = trigger_status
if self.triggered:
agxColor = agxRender.Color(0, 1, 0)
else:
agxColor = agxRender.Color(1, 0, 0)
agxOSG.setDiffuseColor(self.laser_vis, agxColor)
agxOSG.setAlpha(self.laser_vis, 0.5)
def create_water_visual(geo, root):
node = agxOSG.createVisual(geo, root)
diffuse_color = agxRender.Color(0.0, 0.75, 1.0, 1)
ambient_color = agxRender.Color(1, 1, 1, 1)
specular_color = agxRender.Color(1, 1, 1, 1)
agxOSG.setDiffuseColor(node, diffuse_color)
agxOSG.setAmbientColor(node, ambient_color)
agxOSG.setSpecularColor(node, specular_color)
agxOSG.setShininess(node, 120)
agxOSG.setAlpha(node, 0.5)
return node
def add_cylinderShape(MiroSystem, radius, height, density, pos, texture='test.jpg', scale=[1,1], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, color=[0.5, 0.5, 0.5]):
'''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)
scale = scaleLimit(scale)
# Create a cylinder
body_geo = agxCollide.Geometry(agxCollide.Cylinder(radius, height))
body_geo.setName("body")
body_geo.setEnableCollisions(Collide)
body_cylinder = agx.RigidBody(body_geo)
body_cylinder.getMassProperties().setMass(body_geo.calculateVolume()*density)
if Fixed:
body_cylinder.setMotionControl(1)
body_cylinder.setPosition(agxPos)
rotateBody(body_cylinder, rotX=-90)
rotateBody(body_cylinder, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees)
# Collision shape
# if(Collide):
# Visualization shape
body_shape = agxOSG.createVisual(body_cylinder, 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:
if texture not in LOADED_TEXTURES.keys():
agxTex = agxOSG.createTexture(TEXTURE_PATH+texture)
LOADED_TEXTURES.update({texture: agxTex})
agxOSG.setTexture(body_shape, LOADED_TEXTURES[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_cylinder)
return body_cylinder
def create_visual(obj, diffuse_color: Color = None, ambient_color: Color = None,
shininess=None, alpha: float = None):
node = agxOSG.createVisual(obj, root())
if diffuse_color is not None:
agxOSG.setDiffuseColor(node, diffuse_color)
if ambient_color is not None:
agxOSG.setAmbientColor(node, ambient_color)
if shininess is not None:
agxOSG.setShininess(node, shininess)
if alpha is not None:
agxOSG.setAlpha(node, alpha)
return node
def add_rendering(sim):
camera_distance = 0.5
light_pos = agx.Vec4(LENGTH / 2, - camera_distance, camera_distance, 1.)
light_dir = agx.Vec3(0., 0., -1.)
app = agxOSG.ExampleApplication(sim)
app.setAutoStepping(True)
app.setEnableDebugRenderer(False)
app.setEnableOSGRenderer(True)
scene_decorator = app.getSceneDecorator()
light_source_0 = scene_decorator.getLightSource(agxOSG.SceneDecorator.LIGHT0)
light_source_0.setPosition(light_pos)
light_source_0.setDirection(light_dir)
root = app.getRoot()
rbs = sim.getRigidBodies()
for rb in rbs:
name = rb.getName()
node = agxOSG.createVisual(rb, root)
if name == "ground":
agxOSG.setDiffuseColor(node, agxRender.Color.Gray())
elif name == "pusher":
agxOSG.setDiffuseColor(node, agxRender.Color(0.0, 0.0, 1.0, 1.0))
elif "obstacle" in name:
agxOSG.setDiffuseColor(node, agxRender.Color(1.0, 0.0, 0.0, 1.0))
elif "dlo" in name:
agxOSG.setDiffuseColor(node, agxRender.Color(0.0, 1.0, 0.0, 1.0))
elif "bounding_box" in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Burlywood())
else: # Base segments
agxOSG.setDiffuseColor(node, agxRender.Color.Beige())
agxOSG.setAlpha(node, 0.2)
if "goal" in name:
agxOSG.setAlpha(node, 0.2)
scene_decorator = app.getSceneDecorator()
light_source_0 = scene_decorator.getLightSource(agxOSG.SceneDecorator.LIGHT0)
light_source_0.setPosition(light_pos)
light_source_0.setDirection(light_dir)
scene_decorator.setEnableLogo(False)
return app
def add_rendering(sim):
camera_distance = 0.5
light_pos = agx.Vec4(CYLINDER_LENGTH / 2, -camera_distance,
camera_distance, 1.)
light_dir = agx.Vec3(0., 0., -1.)
app = agxOSG.ExampleApplication(sim)
app.setAutoStepping(False)
root = app.getRoot()
rbs = sim.getRigidBodies()
for rb in rbs:
name = rb.getName()
node = agxOSG.createVisual(rb, root, 2.0)
if "ring" in name:
agxOSG.setDiffuseColor(node, COLOR_RING)
elif "ground" in name:
agxOSG.setDiffuseColor(node, COLOR_GROUND)
elif "cylinder" in name:
agxOSG.setDiffuseColor(node, COLOR_CYLINDER)
elif "gripper_right" == name:
agxOSG.setDiffuseColor(node, agxRender.Color(0.0, 0.0, 1.0, 1.0))
elif "gripper_left" == name:
agxOSG.setDiffuseColor(node, agxRender.Color(1.0, 0.0, 0.0, 1.0))
else:
agxOSG.setDiffuseColor(node, agxRender.Color.Beige())
agxOSG.setAlpha(node, 0.2)
app.setEnableDebugRenderer(False)
app.setEnableOSGRenderer(True)
scene_decorator = app.getSceneDecorator()
light_source_0 = scene_decorator.getLightSource(
agxOSG.SceneDecorator.LIGHT0)
light_source_0.setPosition(light_pos)
light_source_0.setDirection(light_dir)
scene_decorator.setEnableLogo(False)
return app
def add_rendering(sim):
app = agxOSG.ExampleApplication(sim)
# Set renderer
app.setAutoStepping(True)
app.setEnableDebugRenderer(False)
app.setEnableOSGRenderer(True)
# Create scene graph for rendering
root = app.getSceneRoot()
rbs = sim.getRigidBodies()
for rb in rbs:
node = agxOSG.createVisual(rb, root)
if rb.getName() == "ground":
agxOSG.setDiffuseColor(node, agxRender.Color(0.8, 0.8, 0.8, 1.0))
elif rb.getName() == "walls":
agxOSG.setDiffuseColor(node, agxRender.Color.Burlywood())
elif rb.getName() == "cylinder":
agxOSG.setDiffuseColor(node, agxRender.Color.DarkGray())
elif rb.getName() == "cylinder_inner":
agxOSG.setDiffuseColor(node, agxRender.Color.LightSteelBlue())
elif rb.getName() == "gripper_0" or rb.getName() == "gripper_1":
agxOSG.setDiffuseColor(node, agxRender.Color(0.1, 0.1, 0.1, 1.0))
elif "dlo" in rb.getName(): # Cable segments
agxOSG.setDiffuseColor(node, agxRender.Color(0.1, 0.5, 0.0, 1.0))
agxOSG.setAmbientColor(node, agxRender.Color(0.2, 0.5, 0.0, 1.0))
elif rb.getName() == "obstacle":
agxOSG.setDiffuseColor(node, agxRender.Color(0.5, 0.5, 0.5, 1.0))
elif rb.getName() == "obstacle_goal":
agxOSG.setDiffuseColor(node, agxRender.Color(0.0, 0.0, 1.0, 1.0))
else:
agxOSG.setDiffuseColor(node, agxRender.Color.Beige())
agxOSG.setAlpha(node, 0.0)
# Set rendering options
scene_decorator = app.getSceneDecorator()
scene_decorator.setEnableLogo(False)
scene_decorator.setBackgroundColor(agxRender.Color(1.0, 1.0,1.0, 1.0))
return app
def add_rendering(sim):
camera_distance = 0.5
light_pos = agx.Vec4(LENGTH / 2, -camera_distance, camera_distance, 1.)
light_dir = agx.Vec3(0., 0., -1.)
app = agxOSG.ExampleApplication(sim)
app.setAutoStepping(False)
app.setEnableDebugRenderer(False)
app.setEnableOSGRenderer(True)
root = app.getSceneRoot()
rbs = sim.getRigidBodies()
for rb in rbs:
name = rb.getName()
node = agxOSG.createVisual(rb, root)
if name == "ground":
agxOSG.setDiffuseColor(node, agxRender.Color.Gray())
elif "gripper_left" in name and "base" not in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Red())
elif "gripper_right" in name and "base" not in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Blue())
elif "dlo" in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Green())
else:
agxOSG.setDiffuseColor(node, agxRender.Color.Beige())
agxOSG.setAlpha(node, 0.5)
if "goal" in name:
agxOSG.setAlpha(node, 0.2)
scene_decorator = app.getSceneDecorator()
light_source_0 = scene_decorator.getLightSource(
agxOSG.SceneDecorator.LIGHT0)
light_source_0.setPosition(light_pos)
light_source_0.setDirection(light_dir)
scene_decorator.setEnableLogo(False)
return app
def createBeam(self, pos, length, rotY):
agxSim = agxPython.getContext().environment.getSimulation()
agxApp = agxPython.getContext().environment.getApplication()
agxRoot = agxPython.getContext().environment.getSceneRoot()
agxPos = agxVecify(pos)
self.laser_geo = agxCollide.Geometry(agxCollide.Cylinder(0.004, length))
self.laser_geo.setName("body")
# self.laser_geo.setEnableCollisions(False)
self.laser_body = agx.RigidBody(self.laser_geo)
self.laser_body.setMotionControl(1)
self.laser_body.setPosition(agxPos)
rotateBody(self.laser_body, rotY=rotY, rotDegrees=False)
# Visualization shape
self.laser_vis = agxOSG.createVisual(self.laser_body, agxRoot)
agxOSG.setAlpha(self.laser_vis, 0.5)
agxSim.add(self.laser_body)
agxSim.add(self)
def __init__(self, ground: agxCollide.Geometry, rov, depth):
"""
Args:
ground:
rov:
depth:
"""
super().__init__()
self.setMask(ContactEventListener.CONTACT)
b = agxCollide.Box(.1, .1, depth)
self.beam = Geometry(b)
# print(self.beam.getShapes(),self.beam)
self.beam.setPosition(0, 0, -depth)
self.beam.setSensor(True)
self.setFilter(GeometryFilter(self.beam, ground))
color = agxRender.Color.IndianRed()
node = agxOSG.createVisual(self.beam, demoutils.root())
agxOSG.setDiffuseColor(node, color)
agxOSG.setAmbientColor(node, agx.Vec4f(1))
agxOSG.setShininess(node, 120)
agxOSG.setAlpha(node, 0.6)
self.ground = ground.getShape().asHeightField()
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 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
