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 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 rotateBody(body, rotX=0, rotY=0, rotZ=0, rotOrder=['x', 'y', 'z'], rotAngle=0, rotAxis=[0,1,0], rotDegrees=True):
agxRotAxis = agxVecify(rotAxis)
agxRotAxis.normalize()
if(rotDegrees):
rotX = np.deg2rad(rotX)
rotY = np.deg2rad(rotY)
rotZ = np.deg2rad(rotZ)
rotAngle = np.deg2rad(rotAngle)
if rotAngle:
q = agx.Quat(-rotAngle, agxRotAxis)
body.setRotation(body.getRotation()*q)
for dim in rotOrder:
angle = (dim == 'x')*-rotX + (dim == 'y')*-rotY + (dim == 'z')*-rotZ
if angle:
axis = agxVecify([(dim == 'x')*1, (dim == 'y')*1, (dim == 'z')*1])
q = agx.Quat(angle, axis)
body.setRotation(body.getRotation()*q)
def rotateVector(vec, rotAngle=0, rotAxis=[0,1,0], rotDegrees=True, transform=True):
vec = agxVecify(vec, transform)
if(rotDegrees):
rotAngle = np.deg2rad(rotAngle)
agxRotAxis = agxVecify(rotAxis)
agxRotAxis.normalize()
q = agx.Quat(-rotAngle, agxRotAxis)
rotvec = q*vec
rotvec = [rotvec.x(), rotvec.y(), rotvec.z()]
if transform:
rotvec = xyzTransform(rotvec, reverse=True)
return np.array(rotvec)
def rotateBodyExp(body, rotX=0, rotY=0, rotZ=0, rotOrder=['x', 'y', 'z'], rotAngle=0, rotAxis=[0,1,0], rotDegrees=True):
agxRotAxis = agxVecify(rotAxis)
agxRotAxis.normalize()
if(rotDegrees):
rotX = np.deg2rad(rotX)
rotY = np.deg2rad(rotY)
rotZ = np.deg2rad(rotZ)
rotAngle = np.deg2rad(rotAngle)
if rotAngle:
q = agx.Quat(rotAngle, agxRotAxis)
body.setRotation(q*body.getRotation())
agx_ex = agxVecify([1,0,0])
agx_ey = agxVecify([0,1,0])
agx_ez = agxVecify([0,0,1])
for dim in rotOrder:
angle = (dim == 'x')*rotX - (dim == 'y')*rotY - (dim == 'z')*rotZ
if angle:
axis = (dim == 'x')*agx_ex + (dim == 'y')*agx_ey + (dim == 'z')*agx_ez
q = agx.Quat(angle, axis)
body.setRotation(body.getRotation()*q)
def seesaw(sim, root, pos, angle, h=0.08):
d = 0.8
# Sides
dims = [0.6*(width/14), 0.15*(width/14), h*3/2]
pos_s = [pos[0]+(d/2+0.3)*np.cos(angle)*(width/14), pos[1]+(d/2+0.3)*np.sin(angle)*(width/14), pos[2]+h/2]
sideP = addboxx(sim, root, dims, pos_s)
dims = [0.6*(width/14), 0.15*(width/14), h*3/2]
pos_s = [pos[0]-(d/2+0.3)*np.cos(angle)*(width/14), pos[1]-(d/2+0.3)*np.sin(angle)*(width/14), pos[2]+h/2]
sideN = addboxx(sim, root, dims, pos_s)
# Main board
dims = [d*(width/14), 0.9*(width/14), 0.004]
pos_s = [pos[0]+0.06*np.sin(angle)*(width/14), pos[1]-0.06*np.cos(angle)*(width/14), pos[2]+h]
board = addboxx(sim, root, dims, pos_s, Fixed=False)
sideP.setRotation(agx.Quat(angle, agx.Vec3(0,0,1)))
sideN.setRotation(agx.Quat(angle, agx.Vec3(0,0,1)))
board.setRotation(agx.Quat(angle, agx.Vec3(0,0,1)))
#Some stops under
bot_tilt = 0.17
dims = [d, 0.43, 0.004]
dif = 0.215*np.cos(bot_tilt)*(width/14)-0.002*np.sin(bot_tilt)*(width/14)
pos_s = [pos[0]+(0.06+dif)*np.sin(angle)*(width/14), pos[1]-(0.06+dif)*np.cos(angle)*(width/14), pos[2]+np.sin(bot_tilt)*dif]
bottom1 = addboxx(sim, root, dims, pos_s, color=agxRender.Color.DarkGray())
pos_s = [pos[0]+(0.06-dif)*np.sin(angle)*(width/14), pos[1]-(0.06-dif)*np.cos(angle)*(width/14), pos[2]+np.sin(bot_tilt)*dif]
bottom2 = addboxx(sim, root, dims, pos_s, color=agxRender.Color.DarkGray())
bottom1.setRotation(agx.Quat( bot_tilt, agx.Vec3(1,0,0)))
bottom1.setRotation(bottom1.getRotation()*agx.Quat(angle, agx.Vec3(0,0,1)))
bottom2.setRotation(agx.Quat(-bot_tilt, agx.Vec3(1,0,0)))
bottom2.setRotation(bottom2.getRotation()*agx.Quat(angle, agx.Vec3(0,0,1)))
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3( np.cos(angle),np.sin(angle),0))
hf.setCenter(agx.Vec3(pos[0]+(d/2)*np.cos(angle)*(width/14), pos[1]+(d/2)*np.sin(angle)*(width/14), pos[2]+h))
axleP = agx.Hinge(hf, board, sideP)
sim.add(axleP)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3( np.cos(angle),np.sin(angle),0))
hf.setCenter(agx.Vec3(pos[0]*(width/14)-(d/2)*np.cos(angle)*(width/14), pos[1]*(width/14)-(d/2)*np.sin(angle)*(width/14), pos[2]+h))
axleN = agx.Hinge(hf, board, sideN)
sim.add(axleN)
def __init__(self, rb: agx.RigidBody):
self.instance = rb
self.name = rb.getName() if rb else '[None]'
self.id = rb.getId() if rb else -1
self.motion_control = rb.getMotionControl() if rb else agx.RigidBody.STATIC
self.mass = rb.getMassProperties().getMass() if rb else 0.0
self.inertia = rb.getMassProperties().getPrincipalInertiae() if rb else agx.Vec3()
self.cm_offset = rb.getCmFrame().getLocalTranslate() if rb else agx.Vec3()
self.linear_velocity = rb.getVelocity() if rb else agx.Vec3()
self.angular_velocity = rb.getAngularVelocity() if rb else agx.Vec3()
self.linear_velocity_damping = rb.getLinearVelocityDamping() if rb else agx.Vec3f()
self.angular_velocity_damping = rb.getAngularVelocityDamping() if rb else agx.Vec3f()
self.force = rb.getForce() if rb else agx.Vec3()
self.torque = rb.getTorque() if rb else agx.Vec3()
self.world_position = rb.getPosition() if rb else agx.Vec3()
self.world_rotation = rb.getRotation() if rb else agx.Quat()
self.geometries = []
if rb is None:
return
for geometry in rb.getGeometries():
self.geometries.append( GeometryInfo( geometry ) )
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 __init__(self,
sim: agxSDK.Simulation,
world_position: agx.Vec3,
world_direction: agx.Vec3,
num_side_rays: int,
rad_range_side: int,
max_length: float,
rb_origin: agx.RigidBody = None,
draw_lines: bool = False):
'''
Creates the lidar sensor. The Lidar sensor is placed at the given world_position and
directed towards the specified world_direction. It always has at least one ray in the world direction.
:param sim: Simulation that the Lines and Listeners are added to
:param world_position: World position of the lidar
:param world_direction: World direction of the lidar
:param num_side_rays: Number of rays created on either side of the middle ray. Can be 0.
:param rad_range_side: The range within the side rays are created.
:param max_length: The maximum length of the lidar rays
:param rb_origin: Rigidbody to lock the lidar body to. Will lock to world if None
:param draw_lines: debug rendering of the rays
'''
super().__init__(agxSDK.StepEventListener.PRE_COLLIDE)
geom = agxCollide.Geometry(agxCollide.Box(0.1, 0.1, 0.1))
geom.setName("lidar_geom")
geom.setSensor(True)
lidar_body = agx.RigidBody(geom)
lidar_body.setMotionControl(agx.RigidBody.KINEMATICS)
lidar_body.setRotation(agx.Quat(agx.Vec3().Z_AXIS(), world_direction))
lidar_body.setPosition(world_position)
if rb_origin is not None:
self._relative_transform = lidar_body.getTransform() * rb_origin.getTransform().inverse()
rays_dict = collections.OrderedDict()
delta = 0
start = 0
if num_side_rays > 0:
delta = rad_range_side/num_side_rays
start = -rad_range_side
self.delta=delta
for i in range(2*num_side_rays + 1):
angle = start + i * delta
z = max_length*math.cos(angle)
y = max_length*math.sin(angle)
ray = agxCollide.Geometry(agxCollide.Line(agx.Vec3(0), agx.Vec3(0, y, z)))
ray.setSensor(True)
ray.setEnableSerialization(False)
ray.addGroup("LidarGeom")
ray.setName("Ray")
ray.setEnableCollisions(geom, False)
lidar_body.add(ray)
rays_dict[ray.getUuid()] = [ray, max_length]
if i == num_side_rays:
self.midRay = ray
self.cel = LidarContactSensor(lidar_body, rays_dict, max_length, rb_origin)
sim.add(self.cel)
sim.add(lidar_body)
self._lidar_body = lidar_body
self._rb_origin = rb_origin
self._rays_dict = rays_dict
self._max_length = max_length
self._draw_lines = draw_lines
render_manager = sim.getRenderManager()
if render_manager and self._draw_lines:
render_manager.disableFlags(agxRender.RENDER_GEOMETRIES)
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 createBucket(self, spec):
length = spec['length'] if 'length' in spec else default['length']
width = spec['width'] if 'width' in spec else default['width']
height = spec['height'] if 'height' in spec else default['height']
thickness = spec['thickness'] if 'thickness' in spec else default[
'thickness']
topFraction = spec[
'topFraction'] if 'topFraction' in spec else default['topFraction']
def createSlopedSides(length, height, thickness, topFraction):
fracLength = (1.0 - topFraction) * length
points = [
agx.Vec3(0.0, 0.0, -thickness),
agx.Vec3(-2.0 * fracLength, 0.0, -thickness),
agx.Vec3(0.0, 2.0 * height, -thickness),
agx.Vec3(0.0, 0.0, thickness),
agx.Vec3(-2.0 * fracLength, 0.0, thickness),
agx.Vec3(0.0, 2.0 * height, thickness)
]
vec3Vector = agx.Vec3Vector()
for point in points:
vec3Vector.append(point)
side = agxUtil.createConvexFromVerticesOnly(vec3Vector)
return side
def createCuttingEdge(length, width, height, thickness, topFraction):
fracLength = (1.0 - topFraction) * length
tanSlope = fracLength / height
edgeLength = tanSlope * thickness
points = [
agx.Vec3(0.0, -width, 0.0),
agx.Vec3(2.0 * edgeLength, -width, 0.0),
agx.Vec3(2.0 * edgeLength, -width, thickness * 2.0),
agx.Vec3(0.0, width, 0.0),
agx.Vec3(2.0 * edgeLength, width, 0.0),
agx.Vec3(2.0 * edgeLength, width, thickness * 2.0)
]
vec3Vector = agx.Vec3Vector()
for point in points:
vec3Vector.append(point)
edge = agxUtil.createConvexFromVerticesOnly(vec3Vector)
return edge
fracLength = (1.0 - topFraction) * length
tanSlope = fracLength / height
edgeLength = tanSlope * thickness
bucket = agx.RigidBody()
bottomPlate = agxCollide.Geometry(
agxCollide.Box(length - edgeLength, width, thickness))
leftSide = agxCollide.Geometry(
agxCollide.Box(length * topFraction, height, thickness))
rightSide = agxCollide.Geometry(
agxCollide.Box(length * topFraction, height, thickness))
backPlate = agxCollide.Geometry(
agxCollide.Box(height, width + thickness, thickness))
topPlate = agxCollide.Geometry(
agxCollide.Box(length * topFraction, width, thickness))
leftSlopedSide = agxCollide.Geometry(
createSlopedSides(length, height, thickness, topFraction))
rightSlopedSide = agxCollide.Geometry(
createSlopedSides(length, height, thickness, topFraction))
edge = agxCollide.Geometry(
createCuttingEdge(length, width, height, thickness, topFraction))
bucket.add(bottomPlate)
bucket.add(leftSide)
bucket.add(rightSide)
bucket.add(backPlate)
bucket.add(topPlate)
bucket.add(leftSlopedSide)
bucket.add(rightSlopedSide)
bucket.add(edge)
bottomPlate.setLocalPosition(edgeLength, 0.0, -height + thickness)
leftSide.setLocalRotation(agx.Quat(agx.PI_2, agx.Vec3.X_AXIS()))
leftSide.setLocalPosition(length * (1.0 - topFraction), width, 0.0)
rightSide.setLocalRotation(agx.Quat(agx.PI_2, agx.Vec3.X_AXIS()))
rightSide.setLocalPosition(length * (1.0 - topFraction), -width, 0.0)
backPlate.setLocalRotation(agx.Quat(agx.PI_2, agx.Vec3.Y_AXIS()))
backPlate.setLocalPosition(length + thickness, 0.0, 0.0)
topPlate.setLocalPosition(length * (1.0 - topFraction), 0.0,
height - thickness)
leftSlopedSide.setLocalRotation(agx.Quat(agx.PI_2, agx.Vec3.X_AXIS()))
leftSlopedSide.setLocalPosition(length * (1.0 - topFraction * 2.0),
width, -height)
rightSlopedSide.setLocalRotation(agx.Quat(agx.PI_2, agx.Vec3.X_AXIS()))
rightSlopedSide.setLocalPosition(length * (1.0 - topFraction * 2.0),
-width, -height)
edge.setLocalPosition(-length, 0.0, -height)
cuttingEdge = agx.Line(agx.Vec3(-length, width, -height + thickness),
agx.Vec3(-length, -width, -height + thickness))
topEdge = agx.Line(agx.Vec3((1.0 - topFraction * 2.0), width, height),
agx.Vec3((1.0 - topFraction * 2.0), -width, height))
forwardVector = agx.Vec3(-1.0, 0.0, 0.0)
return cuttingEdge, topEdge, forwardVector, bucket
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 obstacles(sim, root, h):
#start plattform
dims = [1.5, 1.5, 0.06]
pos = [-6, 0, h + dims[2] / 2]
startbox = addboxx(sim, root, dims, pos)
#timer
timer = Timer(startbox)
sim.add(timer)
dims = [0.1, 1.5, 0.3]
pos = [-6.75, 0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.1, 1.5, 0.3]
pos = [-5.25, 0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [1.6, 0.1, 0.3]
pos = [-6, -0.75, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
# wall
dims = [4.0, 0.1, 0.22]
pos = [-3.3, -0.6, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
# Random stuff in the first quarter
dims = [0.2, 0.2, 0.8]
pos = [-4, 0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.4, 0.25, 0.2]
pos = [-3, 1.5, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
for i in range(30):
x = -0.5 - random.random() * 4.75
y = -0.5 + random.random() * 6
dims = [
random.random() * 0.6,
random.random() * 0.6,
random.random() * 0.6
]
pos = agx.Vec3(x, y, 0)
if pos.length() < 1.5:
pos.setLength(1.5 + random.random() * 3.75)
if pos.length() > 7.0:
pos.setLength(1.5 + random.random() * 5.5)
pos.set(h + dims[2] / 2, 2)
addboxx(sim, root, dims, pos)
# Pole in the middle with walls around
dims = [0.28, 1.4]
pos = [0, 0, h + dims[1] / 2]
can = addcylinder(sim, root, dims, pos, texture='schrodbull.png')
can.setRotation(agx.Quat(np.pi / 2, agx.Vec3(1, 0, 0)))
pos = [0, 0, h + dims[1]]
dims = [0.28, 0.025]
pos[2] = pos[2] + dims[1] / 2
lid = addcylinder(sim, root, dims, pos, texture='sodacan_lid.png')
lid.setRotation(agx.Quat(np.pi / 2, agx.Vec3(1, 0, 0)))
dims = [0.3, 2.0, 0.4]
pos = [-1.15, 0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [2.6, 0.3, 0.4]
pos = [0, -1.15, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.3, 2.0, 0.4]
pos = [1.15, 0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.3, 7.0, 0.4]
pos = [0, 3.5, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
# Seesaw board
dims = [2.1, 0.25, 0.3]
pos = [2.1, 0.4, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
seesaw(sim, root, [3.75, 0.9, h], -0.85 * np.pi, h=0.1)
dims = [0.5, 3.8, 0.18]
pos = [4.8, 3.15, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
# Ballroom walls
dims = [0.25, 4.3, 0.3]
pos = [6.0, 0.0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.25, 1.0, 0.3]
pos = [2.1, -1.0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.25, 1.4, 0.3]
pos = [3.0, -0.4, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.25, 2.4, 0.3]
pos = [2.5, -3.0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [1.1, 0.25, 0.3]
pos = [3.1, -3.1, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.66, 0.25, 0.3]
pos = [2.3, -1.65, h + dims[2] / 2]
wall_1 = addboxx(sim, root, dims, pos)
dims = [2.8, 0.25, 0.3]
pos = [3.95, -2.0, h + dims[2] / 2]
wall_2 = addboxx(sim, root, dims, pos)
dims = [3.91, 0.25, 0.3]
pos = [4.65, -3.45, h + dims[2] / 2]
wall_3 = addboxx(sim, root, dims, pos)
wall_1.setRotation(agx.Quat(-np.pi / 4, agx.Vec3(0, 0, 1)))
wall_2.setRotation(agx.Quat(-np.pi / 4, agx.Vec3(0, 0, 1)))
wall_3.setRotation(agx.Quat(np.pi / 4, agx.Vec3(0, 0, 1)))
# Ballroom balls
for i in range(200):
x = 4.0 + random.random() * 2.8
y = 0.75 - random.random() * 2.5
rad = 0.025 + random.random() * 0.075
pos = agx.Vec3(x, y, h + rad + 3 * random.random() * rad)
addball(sim, root, rad, pos, Fixed=False)
# Climbing ramp
dx = 0.8
bot_tilt = 0.0445
dims = [dx, 2.5, 0.6]
bot_tilt = np.arcsin(0.45 / (2.5 * 4))
dif = dims[1] / 2 * np.cos(bot_tilt) - 0.002 * np.sin(bot_tilt)
dh = 2 * np.sin(bot_tilt) * dif
for i in range(4):
angle = (i + 1) * np.pi
pos = [2 - dx * i, -4.7 - 0.015 * (-1)**i, -0.3 + h + (i + 1 / 2) * dh]
hip = addboxx(sim, root, dims, pos)
hip.setRotation(agx.Quat(bot_tilt, agx.Vec3(1, 0, 0)))
hip.setRotation(hip.getRotation() * agx.Quat(angle, agx.Vec3(0, 0, 1)))
addboxx(sim, root, [2 * dx, 1.1, dims[2]], [
2 - dx * (i + 1 / 2), -4.7 - 1.8 * ((-1)**i), -0.3 + h +
(i + 1) * dh
])
# Bridge boxes
dims = [1.5, 1.8, 0.45]
pos = [-1.5, -3.25, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [1.5, 1.8, 0.45]
pos = [-4.5, -3.25, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
# Bridge part
bridge = addboxx(sim, root, [1.5, 0.5, 0.08],
[-3.0, -3.25, h + 0.45 - 0.04])
timer.addCheckpoint(bridge)
# Swinging ball over bridge
rad = 0.4
pendulum = addball(sim,
root,
rad, [-3.0, -3.25, h + 0.45 + rad + 0.01],
Fixed=False)
pendulum.setVelocity(agx.Vec3(0, 5, 0))
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(1, 0, 0))
hf.setCenter(agx.Vec3(-3.0, -3.25, 3.0 + h + 0.45 + rad + 0.01))
axleP = agx.Hinge(hf, pendulum)
sim.add(axleP)
# addboxx(sim, root, [1.5, 0.5, 0.08], )
# final ramp
addboxx(sim, root, [0.1, 2.5, 0.5], [-6.5, -2.05, h + 0.5 / 2])
addboxx(sim, root, [0.1, 2.5, 0.5], [-5.5, -2.05, h + 0.5 / 2])
hip = addboxx(sim, root, [0.9, 1.5, 0.1],
[-6.0, -1.53, h + 0.026 + np.sin(0.15) * 1.5 / 2])
hip.setRotation(agx.Quat(0.15, agx.Vec3(1, 0, 0)))
addboxx(sim, root, [0.9, 2.0, 0.2], [-6.0, -2.25, h + 0.075])
addboxx(sim, root, [1.4, 0.8, 0.2], [-5.8, -3.7, h + 0.45 - 0.1])
hip = addboxx(sim, root, [0.9, 1.5, 0.1],
[-6.0, -3.0, h + 0.026 + np.sin(0.15) * 1.5 / 2])
hip.setRotation(agx.Quat(-0.15, agx.Vec3(1, 0, 0)))