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 buildArena(arena_pos):
sim = agxPython.getContext().environment.getSimulation()
app = agxPython.getContext().environment.getApplication()
root = agxPython.getContext().environment.getSceneRoot()
arena_size = [width, width, 0.2]
h = 0.35
floor = agx.RigidBody(
agxCollide.Geometry(
agxCollide.Box(arena_size[0] / 2, arena_size[1] / 2,
arena_size[2] / 2)))
floor.setPosition(arena_pos[0], arena_pos[1],
arena_pos[2] - arena_size[2] / 2)
floor.setMotionControl(1)
sim.add(floor)
agxOSG.setDiffuseColor(agxOSG.createVisual(floor, root),
agxRender.Color.Gray())
# Octagon sides
sides = 8
skip_sides = [9]
side_len = width / (1 + np.sqrt(2)) + arena_size[2] / 2 / np.sqrt(2)
base_pos = agx.Vec3(arena_pos[0], arena_pos[1],
arena_pos[2] - arena_size[2] / 2 + h / 2)
for w in range(sides):
if w not in skip_sides:
theta = -w * np.pi / 4
rot = agx.Quat(theta, agx.Vec3(0, 0, 1))
rot_pos = agx.Vec3(
np.sin(theta) * width / 2, -np.cos(theta) * width / 2, 0)
wall = agx.RigidBody(
agxCollide.Geometry(
agxCollide.Box(side_len / 2, arena_size[2] / 2, h / 2)))
wall.setPosition(base_pos + rot_pos)
wall.setMotionControl(1)
wall.setRotation(rot)
sim.add(wall)
agxOSG.setDiffuseColor(agxOSG.createVisual(wall, root),
agxRender.Color.DarkGray())
# Ramp up to the course
ramp_dim = [1.4, side_len, 0.2] # *np.cos(np.pi/4)
ramp = agx.RigidBody(
agxCollide.Geometry(
agxCollide.Box(ramp_dim[0] / 2, ramp_dim[1] / 2, ramp_dim[2] / 2)))
theta = -np.arcsin(ramp_dim[2] / ramp_dim[0]) / 2
ramp.setPosition(
arena_pos[0] - arena_size[0] / 2 - ramp_dim[0] / 2 * np.cos(theta) -
ramp_dim[2] / 2 * np.sin(theta), arena_pos[1],
arena_pos[2] - arena_size[2] * 3 / 4) # +arena_size[1]/2-ramp_dim[1]/2
ramp.setRotation(agx.Quat(theta, agx.Vec3(0, 1, 0)))
ramp.setMotionControl(1)
sim.add(ramp)
agxOSG.setDiffuseColor(agxOSG.createVisual(ramp, root),
agxRender.Color.Gray())
obstacles(sim, root, arena_pos)
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)
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, 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 create_fenders(self, fender_material, r_fender, half_width, half_height, half_length):
fender_color = agxRender.Color.Black()
fender = agxCollide.Geometry(agxCollide.Capsule(0.2, half_width * 2))
fender.setPosition(half_length, 0, 0)
self.m_body.add(fender)
agxOSG.setDiffuseColor(agxOSG.createVisual(fender, demoutils.root()), fender_color)
fender.setMaterial(fender_material)
def create_visual(self, color: agxRender.Color = agxRender.Color.Gold()):
"""
Create visual given color.
Arguments:
color: agxRender.Color, agx.Vec4f - color of this tool
"""
from .environment import root
node = agxOSG.createVisual(self, root())
agxOSG.setDiffuseColor(node, color)
return node
def addball(sim, root, rad, pos, Fixed=True):
if type(pos) == type([]):
pos = agx.Vec3(pos[0], pos[1], pos[2])
ball = agx.RigidBody( agxCollide.Geometry( agxCollide.Sphere(rad)))
ball.setPosition(pos)
if(Fixed):
ball.setMotionControl(1)
sim.add(ball)
agxOSG.setDiffuseColor(agxOSG.createVisual(ball, root), agxRender.Color.Red())
return ball
def addboxx(sim, root, dims, pos, Fixed=True, color = agxRender.Color.Red()):
if type(pos) == type([]):
pos = agx.Vec3(pos[0], pos[1], pos[2])
boxx = agx.RigidBody( agxCollide.Geometry( agxCollide.Box(dims[0]/2, dims[1]/2, dims[2]/2)))
boxx.setPosition(pos)
if(Fixed):
boxx.setMotionControl(1)
sim.add(boxx)
agxOSG.setDiffuseColor(agxOSG.createVisual(boxx, root), color)
return boxx
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 buildArena(sim, root):
width = 14
arena_size = [width, width, 0.2]
arena_pos = [0, 0, -1]
h = 0.7
floor = agx.RigidBody(
agxCollide.Geometry(
agxCollide.Box(arena_size[0] / 2, arena_size[1] / 2,
arena_size[2] / 2)))
floor.setPosition(arena_pos[0], arena_pos[1],
arena_pos[2] - arena_size[2] / 2)
floor.setMotionControl(1)
sim.add(floor)
agxOSG.setDiffuseColor(agxOSG.createVisual(floor, root),
agxRender.Color.Gray())
sides = 8
side_len = width / (1 + np.sqrt(2)) + arena_size[2] / 2 / np.sqrt(2)
base_pos = agx.Vec3(arena_pos[0], arena_pos[1],
arena_pos[2] - arena_size[2] / 2 + h / 2)
for w in range(sides):
theta = -w * np.pi / 4
rot = agx.Quat(theta, agx.Vec3(0, 0, 1))
rot_pos = agx.Vec3(
np.sin(theta) * width / 2, -np.cos(theta) * width / 2, 0)
wall = agx.RigidBody(
agxCollide.Geometry(
agxCollide.Box(side_len / 2, arena_size[2] / 2, h / 2)))
wall.setPosition(base_pos + rot_pos)
wall.setMotionControl(1)
wall.setRotation(rot)
sim.add(wall)
agxOSG.setDiffuseColor(agxOSG.createVisual(wall, root),
agxRender.Color.DarkGray())
obstacles(sim, root, arena_pos[2])
def createLink(self, simulation, mesh, color):
filename = "data/models/robots/Generic/" + mesh + ".obj"
linkRb = agx.RigidBody(filename)
mesh = agxUtil.createTrimeshFromWavefrontOBJ(
filename, agxCollide.Trimesh.NO_WARNINGS, agx.Matrix3x3(),
agx.Vec3())
if mesh is None:
print("Unable to find file: " + filename)
return None
renderData = agxUtil.createRenderDataFromWavefrontOBJ(
filename, agx.Matrix3x3(), agx.Vec3())
mesh.setRenderData(renderData)
render_material = agxCollide.RenderMaterial()
render_material.setDiffuseColor(color)
renderData.setRenderMaterial(render_material)
meshGeom = agxCollide.Geometry(mesh)
linkRb.add(meshGeom)
agxOSG.createVisual(meshGeom,
agxPython.getContext().environment.getSceneRoot())
simulation.add(linkRb)
agxUtil.addGroup(linkRb, self.robotGroupId)
return linkRb
def addcylinder(sim, root, dims, pos, Fixed=True, color = agxRender.Color.Red(), texture=False):
if type(pos) == type([]):
pos = agx.Vec3(pos[0], pos[1], pos[2])
cyl = agx.RigidBody( agxCollide.Geometry( agxCollide.Cylinder(dims[0], dims[1])))
cyl.setPosition(pos)
if(Fixed):
cyl.setMotionControl(1)
sim.add(cyl)
vis_body = agxOSG.createVisual(cyl, root)
if texture:
agxOSG.setTexture(vis_body, texture, True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0)
else:
agxOSG.setDiffuseColor(vis_body, color)
return cyl
def common_settings(self, sim, root, start_pos, visual, color):
"""
Common settings used for all bricks but that must be done after init
"""
self.brick_rb.getMassProperties().setMass(self.props.mass)
self.brick_rb.setPosition(start_pos)
self.set_amor_thresholds(1.e-3)
sim.add(self.brick_rb)
for listener in self.listeners:
sim.add(listener)
if visual:
n = agxOSG.createVisual(self.brick_rb, root)
agxOSG.setDiffuseColor(n, color)
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_table(sim, root, height, length, width, material, visual):
""" Just a flat table """
table = agx.RigidBody()
table.setMotionControl(agx.RigidBody.STATIC)
table.setName("table")
table_geom = agxCollide.Geometry(
agxCollide.Box(length / 2, width / 2, height / 2))
table_geom.setName("table")
table.add(table_geom)
table_geom.setPosition(0, 0, -height / 2)
table_geom.setMaterial(material)
sim.add(table)
if visual:
m = agxOSG.createVisual(table, root)
agxOSG.setDiffuseColor(m, agxRender.Color.SaddleBrown())
return table
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):
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(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 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 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 __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 __init__(self, controller):
super().__init__()
self.controller = controller
width = 2 * 1.5
right_fender = width * 0.25
length = 10 - right_fender * 0.4
ship_color = agxRender.Color.LightYellow()
self.m_propulsion_force = 0
self.m_turn_fraction = 0.1
self.m_turn = 0
self.m_propulsion_step = 30
self.m_max_propulsion = 200000
self.m_min_propulsion = -500
self.init(width, length, right_fender)
self.m_body.getMassProperties().setMass(1000 * 4)
self.m_body.getCmFrame().setLocalTranslate(agx.Vec3(-0.2, 0, 0))
self.m_body.getGeometries()[0].setEnableCollisions(True)
agxOSG.setDiffuseColor(agxOSG.createVisual(self.m_body, demoutils.root()), ship_color)
"""Create callbacks"""
Sec.preCallback(lambda t: self.update_propulsion())
Sec.postCallback(lambda t: self.display_forces(t))
Sec.postCallback(lambda t: self.post(t))
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 buildBot(sim, root, bot_pos, controller='Arrows', drivetrain = 'FWD', color=False):
body_wid = 0.32
body_len = 0.6
body_hei = 0.16
wheel_rad = 0.07
wheel_wid = 0.02
wheel_dmp = -0.02
body = agx.RigidBody( agxCollide.Geometry( agxCollide.Box(body_wid/2, body_len/2, body_hei/2)))
body.setPosition(bot_pos[0], bot_pos[1], bot_pos[2] + body_hei/2 + wheel_rad + wheel_dmp )
# body.setMotionControl(1)
body.setRotation(agx.Quat(np.pi, agx.Vec3(0,0,1)))
sim.add(body)
vis_body = agxOSG.createVisual(body, root)
if color:
agxOSG.setDiffuseColor(vis_body, color)
else:
agxOSG.setTexture(vis_body, 'flames.png', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0)
wheelLF = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(wheel_rad, wheel_wid)))
wheelLF.setPosition(bot_pos[0]-(body_wid/2+wheel_wid/2), bot_pos[1]+(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad)
# wheelLF.setMotionControl(1)
wheelLF.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(wheelLF)
# agxOSG.setDiffuseColor(agxOSG.createVisual(wheelLF, root), agxRender.Color.Red())
wheelRF = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(wheel_rad, wheel_wid)))
wheelRF.setPosition(bot_pos[0]+(body_wid/2+wheel_wid/2), bot_pos[1]+(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad)
# wheelRF.setMotionControl(1)
wheelRF.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(wheelRF)
# agxOSG.setDiffuseColor(agxOSG.createVisual(wheelRF, root), agxRender.Color.Red())
wheelLB = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(wheel_rad, wheel_wid)))
wheelLB.setPosition(bot_pos[0]-(body_wid/2+wheel_wid/2), bot_pos[1]-(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad)
# wheelLB.setMotionControl(1)
wheelLB.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(wheelLB)
# agxOSG.setDiffuseColor(agxOSG.createVisual(wheelLB, root), agxRender.Color.Red())
wheelRB = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(wheel_rad, wheel_wid)))
wheelRB.setPosition(bot_pos[0]+(body_wid/2+wheel_wid/2), bot_pos[1]-(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad)
# wheelRB.setMotionControl(1)
wheelRB.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(wheelRB)
# agxOSG.setDiffuseColor(agxOSG.createVisual(wheelRB, root), agxRender.Color.Red())
for wheel in [wheelLB, wheelLF, wheelRB, wheelRF]:
vis_body = agxOSG.createVisual(wheel, root)
agxOSG.setTexture(vis_body, 'tire.png', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0)
light_rad = 0.02
light_dep = 0.01
headlightL = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(light_rad, light_dep)))
headlightL.setPosition( bot_pos[0] + 0.79*body_wid/2, bot_pos[1] + body_len/2+light_dep/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp)
# headlightL.setMotionControl(1)
# headlightL.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(headlightL)
agxOSG.setTexture(agxOSG.createVisual(headlightL, root), 'light.png', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,1,0))
hf.setCenter(agx.Vec3( bot_pos[0] + 0.79*body_wid/2, bot_pos[1] + body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp ))
HLL = agx.Hinge(hf, body, headlightL)
sim.add(HLL)
headlightR = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(light_rad, light_dep)))
headlightR.setPosition(bot_pos[0] -0.79*body_wid/2, bot_pos[1] + body_len/2+light_dep/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp )
# headlightR.setMotionControl(1)
# headlightL.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(headlightR)
agxOSG.setTexture(agxOSG.createVisual(headlightR, root), 'light.png', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,1,0))
hf.setCenter(agx.Vec3(bot_pos[0] -0.79*body_wid/2, bot_pos[1] + body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp ))
HLR = agx.Hinge(hf, body, headlightR)
sim.add(HLR)
light_wid = 0.012
light_hei = 0.02
light_dep = 0.003
taillightL = agx.RigidBody(agxCollide.Geometry( agxCollide.Box(light_wid, light_dep, light_hei)))
taillightL.setPosition(bot_pos[0] -0.79*body_wid/2,bot_pos[1] -(body_len/2+light_dep/2), bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp)
# taillightL.setMotionControl(1)
# headlightL.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(taillightL)
agxOSG.setDiffuseColor(agxOSG.createVisual(taillightL, root), agxRender.Color.Red())
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,1,0))
hf.setCenter(agx.Vec3(bot_pos[0] -0.79*body_wid/2, bot_pos[1] -body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp + light_hei/3))
TLL_hi = agx.Hinge(hf, body, taillightL)
sim.add(TLL_hi)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,1,0))
hf.setCenter(agx.Vec3(bot_pos[0] -0.79*body_wid/2, bot_pos[1] -body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp - light_hei/3))
TLL_low = agx.Hinge(hf, body, taillightL)
sim.add(TLL_low)
taillightR = agx.RigidBody(agxCollide.Geometry( agxCollide.Box(light_wid, light_dep, light_hei)))
taillightR.setPosition( bot_pos[0] + 0.79*body_wid/2,bot_pos[1] -(body_len/2+light_dep/2), bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp)
# taillightR.setMotionControl(1)
# headlightL.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(taillightR)
agxOSG.setDiffuseColor(agxOSG.createVisual(taillightR, root), agxRender.Color.Red())
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,1,0))
hf.setCenter(agx.Vec3( bot_pos[0] + 0.79*body_wid/2,bot_pos[1]-body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp + light_hei/3))
TLR = agx.Hinge(hf, body, taillightR)
sim.add(TLR)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,1,0))
hf.setCenter(agx.Vec3( bot_pos[0] + 0.79*body_wid/2,bot_pos[1]-body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp - light_hei/3))
TLR = agx.Hinge(hf, body, taillightR)
sim.add(TLR)
windangle = np.pi/4
windshield = agx.RigidBody( agxCollide.Geometry( agxCollide.Box(0.9*body_wid/2, 0.005, body_hei/3)))
windshield.setPosition(bot_pos[0], bot_pos[1]+body_len/5, bot_pos[2] + body_hei + wheel_rad + wheel_dmp + np.cos(windangle)*body_hei/3)
# windshield.setTorque(0,0,100)
# windshield.setMotionControl(2)
windshield.setRotation(agx.Quat(windangle, agx.Vec3(1,0,0)))
sim.add(windshield)
agxOSG.setTexture(agxOSG.createVisual(windshield, root), 'windshield.jpg', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,0,1))
hf.setCenter(agx.Vec3(-body_wid/3, body_len/5, bot_pos[2] + body_hei + wheel_rad + wheel_dmp))
windh1 = agx.Hinge(hf, body, windshield)
sim.add(windh1)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,0,1))
hf.setCenter(agx.Vec3(body_wid/3, body_len/5, bot_pos[2] + body_hei + wheel_rad + wheel_dmp))
windh2 = agx.Hinge(hf, body, windshield)
sim.add(windh2)
x_ax = agx.Vec3(1,0,0)
y_ax = agx.Vec3(0,1,0)
z_ax = agx.Vec3(0,0,1)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(-1,0,0))
hf.setCenter(agx.Vec3(bot_pos[0]-body_wid/2, bot_pos[1]+(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad))
axleLF = agx.Hinge(hf, body, wheelLF)
sim.add(axleLF)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(-1,0,0))
hf.setCenter(agx.Vec3(bot_pos[0]-body_wid/2, bot_pos[1]-(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad))
axleLB = agx.Hinge(hf, body, wheelLB)
sim.add(axleLB)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3( 1,0,0))
hf.setCenter(agx.Vec3(bot_pos[0]+body_wid/2, bot_pos[1]-(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad))
axleRB = agx.Hinge(hf, body, wheelRB)
sim.add(axleRB)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3( 1,0,0))
hf.setCenter(agx.Vec3(bot_pos[0]+body_wid/2, bot_pos[1]+(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad))
axleRF = agx.Hinge(hf, body, wheelRF)
sim.add(axleRF)
wheels = [wheelLF, wheelRF] # default FWD
if drivetrain == 'FWD':
wheels = [wheelLF, wheelRF]
elif drivetrain == 'RWD':
wheels = [wheelLB, wheelRB]
elif drivetrain == 'AWD':
wheels = [wheelLF, wheelRF, wheelLB, wheelRB]
if controller == 'Numpad':
sim.add(WheelControllerNumpad(wheels))
sim.add(Fjoink(body).setHopper(agxSDK.GuiEventListener.KEY_KP_Subtract))
else:
sim.add(WheelControllerArrows(wheels))
sim.add(Fjoink(body))
# return a pointer to the body
return body
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 setup_geometries(self, root, material, visual):
# pylint: disable=too-many-statements, too-many-locals
""" Create bodies and constraints so that we can move the gripper left/right/front/back """
translate_bodies = [agx.RigidBody(), agx.RigidBody()]
move_direction = [agx.Vec3(-1, 0, 0), agx.Vec3(0, -1, 0)]
for i in range(0, 2):
body = translate_bodies[i]
body.setLocalPosition(0, 0, 0)
body.getMassProperties().setMass(self.props.mass)
self.gripper.add(body)
if i == 0:
frame1 = agx.Frame()
frame1.setLocalTranslate(body.getPosition())
frame1.setLocalRotate(
agx.Quat(agx.Vec3(0, 0, -1), move_direction[i]))
prismatic = agx.Prismatic(body, frame1)
else:
frame1 = agx.Frame()
frame2 = agx.Frame()
assert agx.Constraint.calculateFramesFromBody(agx.Vec3(), \
move_direction[i], translate_bodies[0], frame1, body, frame2)
prismatic = agx.Prismatic(translate_bodies[0], frame1, body,
frame2)
self.gripper.add(prismatic)
prismatic.getMotor1D().setLockedAtZeroSpeed(True)
prismatic.getMotor1D().setForceRange(agx.RangeReal(50)) # N
prismatic.getMotor1D().setSpeed(0)
prismatic.getMotor1D().setEnable(True)
prismatic.setSolveType(agx.Constraint.DIRECT_AND_ITERATIVE)
prismatic.setCompliance(1e-15)
self.move_constraints.append(prismatic)
# Create a Cylindrical constraint that can be used for lifting AND rotating
lift_rotate_body = agx.RigidBody()
lift_rotate_body.setLocalPosition(0, 0, 0)
lift_rotate_body.getMassProperties().setMass(self.props.mass)
self.gripper.add(lift_rotate_body)
frame1 = agx.Frame()
frame2 = agx.Frame()
assert agx.Constraint.calculateFramesFromBody( \
agx.Vec3(), agx.Vec3(0, 0, -1), translate_bodies[1], frame1, lift_rotate_body, frame2)
self.cylindrical = agx.CylindricalJoint(translate_bodies[1], frame1,
lift_rotate_body, frame2)
self.cylindrical.setSolveType(agx.Constraint.DIRECT_AND_ITERATIVE)
self.cylindrical.setCompliance(1e-15)
# Enable the motors and set some properties on the motors
for d in [agx.Constraint2DOF.FIRST, agx.Constraint2DOF.SECOND]:
self.cylindrical.getMotor1D(d).setEnable(True)
self.cylindrical.getMotor1D(d).setLockedAtZeroSpeed(True)
self.cylindrical.getMotor1D(d).setSpeed(0)
if d == agx.Constraint2DOF.FIRST:
self.cylindrical.getMotor1D(d).setForceRange(
agx.RangeReal(15)) # N
else:
self.cylindrical.getMotor1D(d).setForceRange(
agx.RangeReal(50)) # Nm
self.gripper.add(self.cylindrical)
# Create the actual fingers that is used for picking up screws.
capsule1 = agxCollide.Geometry(
agxCollide.Capsule(self.props.radius, self.props.height))
capsule1.setLocalRotation(agx.EulerAngles(math.radians(90), 0, 0))
capsule1.setMaterial(material)
capsule2 = capsule1.clone()
capsule1.setLocalPosition(0, -self.props.radius, 0)
capsule2.setLocalPosition(0, self.props.radius, 0)
self.finger1 = agx.RigidBody()
self.finger1.add(capsule1)
self.finger1.add(capsule2)
self.finger2 = self.finger1.clone()
fingers = [self.finger1, self.finger2]
# Create the constraints that is used for open/close the picking device
direction = [1, -1]
self.grip_constraints = []
grip_range = 0.025
self.grip_offs = self.props.radius
for i in range(0, 2):
finger = fingers[i]
finger.setLocalPosition(
direction[i] * (self.grip_offs + self.posref.grip1), 0, 0)
self.gripper.add(finger)
finger.getMassProperties().setMass(self.props.mass)
frame1 = agx.Frame()
frame2 = agx.Frame()
assert agx.Constraint.calculateFramesFromBody( \
agx.Vec3(), agx.Vec3(-1, 0, 0) * direction[i], lift_rotate_body, frame1, finger, frame2)
prismatic = agx.Prismatic(lift_rotate_body, frame1, finger, frame2)
prismatic.getMotor1D().setForceRange(agx.RangeReal(-20, 20))
prismatic.getMotor1D().setLockedAtZeroSpeed(True)
prismatic.getMotor1D().setEnable(True)
prismatic.getRange1D().setRange(
agx.RangeReal(-self.posref.grip1,
grip_range - self.posref.grip1))
prismatic.getRange1D().setEnable(True)
prismatic.setSolveType(agx.Constraint.DIRECT_AND_ITERATIVE)
prismatic.setCompliance(1e-15)
self.gripper.add(prismatic)
self.grip_constraints.append(prismatic)
self.sim.add(self.gripper)
if visual:
agxOSG.createVisual(self.gripper, root)
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
