def __init__(self, material: agx.Material = None):
super().__init__()
servo = agxCollide.Geometry(servo_shape.deepCopy())
servo.setEnableCollisions(False)
snakeapp.create_visual(servo, diffuse_color=agxRender.Color.Black())
self.bottom = agx.RigidBody(
agxCollide.Geometry(bottom_shape.deepCopy()))
self.bottom.add(servo)
snakeapp.create_visual(self.bottom, agxRender.Color.Orange())
self.upper = agx.RigidBody(agxCollide.Geometry(upper_shape.deepCopy()))
snakeapp.create_visual(self.upper, agxRender.Color.Orange())
if material is not None:
self.bottom.getGeometries()[0].setMaterial(material)
self.upper.getGeometries()[0].setMaterial(material)
self.hinge = create_constraint(pos=agx.Vec3(0.0, 0.0007, 0),
axis=agx.agx.Vec3(0, 0, -1),
rb1=self.bottom,
rb2=self.upper,
c=agx.Hinge) # type: agx.Hinge
self.hinge.setCompliance(1E-12)
self.hinge.getMotor1D().setCompliance(1E-10)
self.hinge.getMotor1D().setEnable(True)
self.hinge.getLock1D().setEnable(False)
self.hinge.getRange1D().setRange(-math.pi / 2, math.pi / 2)
self.add(self.bottom)
self.add(self.hinge)
self.add(self.upper)
def __init__(self, material: agx.Material = None):
super().__init__()
visual_geometry = agxCollide.Geometry(intermediate_shape.deepCopy(),
agx.AffineMatrix4x4.rotate(math.pi / 2, 0, 1, 0) *
agx.AffineMatrix4x4.rotate(math.pi, 1, 0, 0))
visual_geometry.setEnableCollisions(False)
snakeapp.create_visual(visual_geometry, agxRender.Color.Orange())
collision_geometry = agxCollide.Geometry(agxCollide.Box(intermediate_bounds),
agx.AffineMatrix4x4.translate(intermediate_len / 2, 0, 0))
self.body = agx.RigidBody()
self.body.add(visual_geometry)
self.body.add(collision_geometry)
self.add(self.body)
sensor_geometry = agxCollide.Geometry(agxCollide.Box(sensor_bounds),
agx.AffineMatrix4x4.translate(intermediate_len / 2, -0.035, 0))
self.sensor = agx.RigidBody(sensor_geometry)
self.add(self.sensor)
if material is not None:
collision_geometry.setMaterial(material)
sensor_geometry.setMaterial(material)
self.merged_body = agx.MergedBody()
self.merged_body.add(agx.MergedBodyEmptyEdgeInteraction(self.body, self.sensor))
snakeapp.add(self.merged_body)
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 create_bodies(position1, position2, size):
b1 = agx.RigidBody()
b2 = agx.RigidBody()
b1.add(agxCollide.Geometry(agxCollide.Box(size[0], size[1], size[2])))
b2.add(agxCollide.Geometry(agxCollide.Box(size[0], size[1], size[2])))
b1.setPosition(agx.Vec3(position1[0], position1[1], position1[2]))
b2.setPosition(agx.Vec3(position2[0], position2[1], position2[2]))
return b1, b2
def create_bodies(position1, position2, size):
# Create first leg section and set position
leg_section1 = agx.RigidBody()
leg_section1.add(
agxCollide.Geometry(agxCollide.Box(size[0], size[1], size[2])))
leg_section1.setPosition(agx.Vec3(position1[0], position1[1],
position1[2]))
# Create second leg section and set position
leg_section2 = agx.RigidBody()
leg_section2.add(
agxCollide.Geometry(agxCollide.Box(size[0], size[1], size[2])))
leg_section2.setPosition(agx.Vec3(position2[0], position2[1],
position2[2]))
return leg_section1, leg_section2
def create_sphere(position: agx.Vec3):
shape = agxCollide.Sphere(0.5)
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
demoutils.create_visual(body)
return body
def create_sphere(position: agx.Vec3, scale):
shape = agxCollide.Sphere(0.5 * scale)
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
oneLegRobotApp.create_visual(body)
return body
def __init__(self, props):
self.brick_rb = agx.RigidBody()
self.brick_rb.setName("duplobrick")
self.height = props.height + props.stud_height #For broadcasting as sensor data
self.props = props
self.listeners = []
self.hole_sensors = []
def setup_scene(i: int):
snake = Snake(NUM_SNAKE_MODULES, pitch_only=False,
with_camera=True) # type: Snake
snake.setPosition(agx.Vec3(0, 0, 0.1))
snakeapp.add(snake)
plane_body = agx.RigidBody(
agxCollide.Geometry(agxCollide.Box(2, 2, 0.1),
agx.AffineMatrix4x4.translate(0, 0, -0.1 / 2)))
plane_body.setMotionControl(agx.RigidBody.STATIC)
snakeapp.create_visual(plane_body, diffuse_color=agxRender.Color.Green())
snakeapp.add(plane_body)
snake_controller = SnakeControl(snake)
if i == FLAPPING:
snake_controller.init_flapping(math.pi / 9.0, math.pi / 9.0, 16.0,
-math.pi * 5.0 / 180.0)
elif i == TURNING:
snake_controller.init_turning(math.pi / 9.0, math.pi * 2.0 / 3.0, 8.0,
0.0, math.pi * 20.0 / 180.0)
elif i == SIDEWINDING:
snake_controller.init_sidewinding(math.pi / 9.0, math.pi * 2.0 / 3.0,
16.0)
elif i == ROLLING:
snake_controller.init_rolling(math.pi / 6.0, math.pi / 6.0, 16.0)
elif i == ROTATING:
snake_controller.init_rotating(math.pi / 6.0, math.pi / 6.0, 16.0)
snakeapp.add_event_listener(snake_controller)
snakeapp.init_camera(eye=agx.Vec3(-1, -1, 0.5),
center=plane_body.getPosition())
def create_box(position: agx.Vec3):
shape = agxCollide.Box(agx.Vec3(0.5, 0.5, 0.5))
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
demoutils.create_visual(body)
return body
def build_rigid_body(geom: agxCollide.Geometry, name: str, pos,
rot) -> agx.RigidBody:
body = agx.RigidBody()
body.add(geom)
body.setName(name)
body.setPosition(*pos)
body.setRotation(agx.EulerAngles(*rot))
return body
def create_cylinder(position: agx.Vec3):
shape = agxCollide.Cylinder(0.5, 1)
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
demoutils.create_visual(body)
body.setRotation(agx.EulerAngles(agx.PI_2, 0, 0))
return body
def create_cylinder(position: agx.Vec3, scale):
shape = agxCollide.Cylinder(0.5 * scale, 1 * scale)
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
oneLegRobotApp.create_visual(body)
body.setRotation(agx.EulerAngles(agx.PI_2, 0, 0))
return body
def create_section(self, x, y, z, rotx, roty, rotz, path):
geometry = agxCollide.Geometry(path.deepCopy(),
agx.AffineMatrix4x4.translate(x, y, z))
geometry.setLocalRotation(agx.EulerAngles(rotx, roty, rotz))
geometry.setEnableCollisions(True)
rigidBody = agx.RigidBody(geometry)
rigidBody.setMotionControl(agx.RigidBody.DYNAMICS)
oneLegRobotApp.create_visual(rigidBody, agxRender.Color.Red())
return rigidBody
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 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 add_first_joint_aft():
geometry = agxCollide.Geometry(first_joint_shape_aft.deepCopy(),
agx.AffineMatrix4x4.translate(0, 0, 200))
geometry.setLocalRotation(agx.EulerAngles(0, 0, math.pi / 2))
print(geometry.getRotation())
geometry.setEnableCollisions(True)
rigidBody = agx.RigidBody(geometry)
oneLegRobotApp.create_visual(rigidBody, agxRender.Color.Red())
oneLegRobotApp.add(rigidBody)
rigidBody.setMotionControl(agx.RigidBody.STATIC)
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_rigid_body_from_obj(name, scale, material):
convexShapes = agxCollide.ConvexRefVector()
convexBuilder = None
agxUtil.createCdFromWavefrontOBJ(name, convexShapes, convexBuilder, scale)
rb = agx.RigidBody()
for c in convexShapes:
assert (c)
g = agxCollide.Geometry(c.asConvex())
if material:
g.setMaterial(material)
rb.add(g)
return rb
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 build_scene1():
# Create a geometry with a plane shape as our 'floor'
floor_geometry = agxCollide.Geometry(agxCollide.Box(agx.Vec3(10, 10, 0.1)))
demoutils.create_visual(floor_geometry)
demoutils.sim().add(floor_geometry) # Add the geometry to the simulation
rb1 = agx.RigidBody() # Create a rigid body
rb1.add(agxCollide.Geometry(agxCollide.Sphere(
0.5))) # Add a geometry with a sphere-shape of radius 0.5
rb1.setPosition(0, 0, 5.0) # Position the sphere somewhere above our plane
demoutils.create_visual(rb1)
demoutils.sim().add(
rb1) # Add the body to the simulation. The geometry will also be added
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 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 create_floor(self):
w = 200
b = 200
h = 10
# floor = agxCollide.Geometry(agxCollide.Box(2.5, 0.5, h), agx.AffineMatrix4x4.translate(0, 0, -h))
floor = agxCollide.Geometry(agxCollide.Box(w, b, h))
floor.setPosition(0, 0, 0)
floor.setRotation(agx.EulerAngles(0, 0, 0))
floor.setEnableCollisions(True)
rigidBody = agx.RigidBody(floor)
rigidBody.setMotionControl(agx.RigidBody.STATIC)
oneLegRobotApp.create_visual(rigidBody,
diffuse_color=agxRender.Color.Green())
return floor
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 build_scene2():
# Create a geometry with a plane shape as our 'floor'
floor_geometry = agxCollide.Geometry(agxCollide.Box(agx.Vec3(10, 10, 0.1)))
demoutils.create_visual(floor_geometry, diffuse_color=Color.Green())
demoutils.sim().add(floor_geometry) # Add the geometry to the simulation
for x in range(-5, 5):
for y in range(-5, 5):
for z in range(1, 8):
rb = agx.RigidBody() # Create a rigid body
rb.add(agxCollide.Geometry(agxCollide.Sphere(
0.2))) # Add a geometry with a sphere-shape of radius 0.2
rb.setPosition(
x + random(), y + random(),
z) # Position the sphere somewhere above our plane
demoutils.create_visual(rb)
demoutils.sim().add(
rb
) # Add the body to the simulation. The geometry will also be added
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 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 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)
