def top_edge_world(self) -> agx.Line:
"""
Top edge in world coordinates.
Returns:
agx.Line - top edge of this tool, in world coordinates
"""
return self._instance.getTopEdgeWorld()\
if self._instance\
else agx.Line( self._tpw( self.top_edge.p1 ), self._tpw( self.top_edge.p2 ) )
def cutting_edge_world(self) -> agx.Line:
"""
Cutting edge in world coordinates.
Returns:
agx.Line - cutting edge of this tool, in world coordinates
"""
return self._instance.getCuttingEdgeWorld()\
if self._instance\
else agx.Line( self._tpw( self.cutting_edge.p1 ), self._tpw( self.cutting_edge.p2 ) )
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
class WheelLoaderL70(WheelLoader):
"""
Wheel loader class that loads 'model_path' and configures
tires and driveline etc. The tires are listed as:
[front left, front right, rear left, rear right]
The driveline is configured with a combustion engine, torque converter,
gear box, center differential, front and rear differentials and
tire hinge actuators. The brake is located on the shaft between the
gear box and the center differential which enables brakes and steering
without jamming.
engine
^
|
|
torque_converter
^
|
|
front_right_tire_actuator gear_box rear_right_tire_actuator
^ ^ ^
| | <-- brake_hinge |
| | |
front_differential <----- center_differential --------> rear_differential
| |
| |
front_left_tire_actuator rear_left_tire_actuator
Examples:
from agxPythonModules.utils.callbacks import KeyboardCallback as Input
from agxPythonModules.models.wheel_loaders import WheelLoaderL70
tire_settings = WheelLoaderL70.default_tire_settings()
tire_settings.stiffness.radial = 1.0E6
keyboard_settings = WheelLoaderL70.default_keyboard_settings()
keyboard_settings.engine.forward.key = ord( 'a' )
keyboard_settings.engine.reverse.key = ord( 'z' )
keyboard_settings.elevate_up.key = Input.KEY_Up
keyboard_settings.elevate_down.key = Input.KEY_Down
wheel_loader1 = WheelLoaderL70( tire_settings = tire_settings,
keyboard_settings = keyboard_settings )
wheel_loader1.setPosition( 0, 5, 0 )
simulation().add( wheel_loader1 )
wheel_loader2 = WheelLoaderL70()
simulation().add( wheel_loader2 )
"""
model_path = 'data/models/wheel_loader_L70.agx'
model_top_edge = agx.Line(agx.Vec3(-1.3, 0.858258, 0.583871),
agx.Vec3(1.3, 0.858258, 0.583871))
model_cutting_edge = agx.Line(agx.Vec3(-1.3, -0.763665, 0.433567),
agx.Vec3(1.3, -0.763665, 0.433567))
model_cutting_dir = agx.Vec3(0, -0.473151, 0.880981).normal()
@classmethod
def default_driveline_settings(cls):
"""
Returns:
Struct - default driveline settings
"""
return Struct({
'engine': {
'displacement_volume': 0.015,
'volumetric_efficiency': 0.9
},
'torque_converter': {
'max_multiplication':
2.0,
'reference_rpm':
1000,
'multiplication_table': [(-0.0001, 0.00), (0.00001, 0.50),
(0.00011, 2.00), (0.00100, 2.00),
(0.20000, 1.10), (0.40000, 1.15),
(0.60000, 1.05), (0.80000, 1.01),
(0.90000, 0.99), (1.00000, 0.98),
(1.00100, 0.98)]
},
'gear_box': {
# Currently only one reverse and one forward gear.
'gear_ratios': [-10.0, 10.0]
},
'center_differential': {
'gear_ratio': 10.0,
# Locking center differential for even torque distribution over
# front and rear tires. Otherwise it's likely to have one tire
# spinning - especially operating in slopes.
'locked': True
},
'front_differential': {
'gear_ratio': 1.0,
'locked': False
},
'rear_differential': {
'gear_ratio': 1.0,
'locked': False
}
})
@classmethod
def default_tire_settings(cls):
"""
Returns:
Struct - default tire settings.
"""
return Struct({
'stiffness': {
'radial': 2.0E6,
'lateral': 4.0E6,
'bending': 1.0E6,
'torsional': 1.0E6
},
'damping_coefficient': {
'radial': 9.0E4,
'lateral': 9.0E4,
'bending': 9.0E4,
'torsional': 9.0E4
}
})
@classmethod
def default_keyboard_controls(cls):
"""
Default keyboard settings:
KEY_Up: forward
KEY_Down: reverse
KEY_Left: steer left
KEY_Right: steer right
'a': bucket elevate up
'z': bucket elevate down
's': bucket tilt up
'x': bucket tilt down
Returns:
Struct - default keyboard controls settings.
"""
return Struct({
'engine': {
'throttle_increase_rate':
2.0, # When pressing throttle - throttle value is this constant times key-down-time.
'throttle_decrease_rate':
4.0, # When releasing throttle - throttle value is one minus this constant times key-released-time.
'forward': {
'key': Input.KEY_Up,
},
'reverse': {
'key': Input.KEY_Down
}
},
'steer_left': {
'key': Input.KEY_Left,
'speed': -1.0,
'acceleration_time': 0.5,
'deceleration_time': 0.5
},
'steer_right': {
'key': Input.KEY_Right,
'speed': 1.0,
'acceleration_time': 0.5,
'deceleration_time': 0.5
},
'elevate_up': {
'key': ord('a'),
'speed': 0.35,
'acceleration_time': 1.0,
'deceleration_time': 1.5
},
'elevate_down': {
'key': ord('z'),
'speed': -0.35,
'acceleration_time': 1.0,
'deceleration_time': 0.5
},
'tilt_up': {
'key': ord('s'),
'speed': -0.35,
'acceleration_time': 0.5,
'deceleration_time': 0.5
},
'tilt_down': {
'key': ord('x'),
'speed': 0.35,
'acceleration_time': 0.5,
'deceleration_time': 0.5
}
})
@classmethod
def default_gamepad_controls(cls, deadzone: float = 0.2):
return Struct({
'engine': {
'invert': True,
'deadzone': deadzone,
'forward': {
'axis': Gamepad.Axis.RightTrigger
},
'reverse': {
'axis': Gamepad.Axis.LeftTrigger
}
},
'steer': {
'axis': Gamepad.Axis.LeftHorizontal,
'deadzone': deadzone,
'invert': False,
'max_speed': 1.0
},
'elevate': {
'axis': Gamepad.Axis.RightVertical,
'deadzone': deadzone,
'invert': True,
'max_speed': 0.5
},
'tilt': {
'axis': Gamepad.Axis.RightHorizontal,
'deadzone': deadzone,
'invert': False,
'max_speed': 0.4
}
})
def __init__(self, **kwargs):
"""
Construct given optional arguments.
Arguments:
driveline_settings: Struct - driveline settings, WheelLoaderL70.default_driveline_settings() is used if not given.
tire_settings: Struct - tire settings, WheelLoaderL70.default_tire_settings() is used if not given.
keyboard_controls: Struct - keyboard controls (e.g., WheelLoaderL70.default_keyboard_controls())
gamepad_controls: Struct - gamepad controls (e.g., WheelLoaderL70.default_gamepad_controls())
"""
if not 'driveline_settings' in kwargs:
kwargs['driveline_settings'] = self.default_driveline_settings()
if not 'tire_settings' in kwargs:
kwargs['tire_settings'] = self.default_tire_settings()
if not 'model_path' in kwargs:
kwargs['model_path'] = self.model_path
super().__init__(**kwargs)
def __init__(self, **kwargs):
"""
Construct tool body.
Arguments:
size: [( agx.Vec3, float )] - list of segment half extents and absolute angle in degrees.
name: str - [Optional] Name of this tool body (default: 'tool_body').
motion_control: agx.RigidBody.MotionControl - [Optional] Motion control of this tool body (default: DYNAMICS).
create_walls: bool - [Optional] Create side walls (default: True).
add_walls: bool - See 'create_walls'.
"""
if not 'size' in kwargs:
raise TypeError(
"'size' list of tuple ( agx.Vec3( segment_half_extent ), angle_degrees ) not given."
)
super().__init__(kwargs.get('name', 'tool_body'))
self.setMotionControl(kwargs.get('motion_control', self.DYNAMICS))
self._instance = None # type: agxTerrain.Shovel
self._left_wall_vertices = agx.Vec3Vector()
self._right_wall_vertices = agx.Vec3Vector()
self._inner_volume_vertices = agx.Vec3Vector()
# At the end, this is the bottom center, or last geometry center position.
self._position = agx.Vec3()
sizes = kwargs['size']
from math import radians, sin, cos
for size in sizes:
he = size[0]
angle = radians(size[1])
thickness = 2.0 * he.x()
box = agxCollide.Geometry(agxCollide.Box(he))
self._position = self._position + agx.Vec3(-he.z() * sin(angle), 0,
-he.z() * cos(angle))
geometry_transform = agx.AffineMatrix4x4.rotate( angle,
agx.Vec3.Y_AXIS() ) *\
agx.AffineMatrix4x4.translate( self._position )
def add_vertices(z_sign: float):
self._left_wall_vertices.append(
agx.Vec3(-he.x(), he.y(), z_sign * he.z()) *
geometry_transform)
self._left_wall_vertices.append(
agx.Vec3(-he.x(),
he.y() + thickness, z_sign * he.z()) *
geometry_transform)
self._right_wall_vertices.append(
agx.Vec3(-he.x(), -he.y(), z_sign * he.z()) *
geometry_transform)
self._right_wall_vertices.append(
agx.Vec3(-he.x(), -he.y() - thickness, z_sign * he.z()) *
geometry_transform)
self._inner_volume_vertices.append(
agx.Vec3(-he.x(), he.y(), z_sign * he.z()) *
geometry_transform)
self._inner_volume_vertices.append(
agx.Vec3(-he.x(), -he.y(), z_sign * he.z()) *
geometry_transform)
# First vertex, add top vertices.
if len(self._left_wall_vertices) == 0:
add_vertices(1.0)
add_vertices(-1.0)
self.add(box, geometry_transform)
self._position = self._position + agx.Vec3(-he.z() * sin(angle), 0,
-he.z() * cos(angle))
# 'create_walls' or 'add_walls' accepted.
if kwargs.get(
'create_walls' if 'create_walls' in kwargs else 'add_walls',
True):
self.add(
agxCollide.Geometry(
agxUtil.createConvexFromVerticesOnly(
self._left_wall_vertices)))
self.add(
agxCollide.Geometry(
agxUtil.createConvexFromVerticesOnly(
self._right_wall_vertices)))
top_he = sizes[0][0]
top_angle = radians(sizes[0][1])
bottom_he = sizes[len(sizes) - 1][0]
bottom_angle = radians(sizes[len(sizes) - 1][1])
self._top_edge = agx.Line(
self.getPosition() +
agx.Vec3(top_he.x() * cos(top_angle), -top_he.y(),
-top_he.x() * sin(top_angle)),
self.getPosition() +
agx.Vec3(top_he.x() * cos(top_angle), top_he.y(),
-top_he.x() * sin(top_angle)))
self._cutting_edge = agx.Line(
self._position +
agx.Vec3(bottom_he.x() * cos(bottom_angle), -bottom_he.y(),
-bottom_he.x() * sin(bottom_angle)), self._position +
agx.Vec3(bottom_he.x() * cos(bottom_angle), bottom_he.y(),
-bottom_he.x() * sin(bottom_angle)))
# use the angle of the last box to estimate the forward vector of the created shovel
self._forward_vector = agx.Vec3(sin(-bottom_angle), 0,
-cos(-bottom_angle))
def top_edge(self):
return agx.Line( self.m_top_edge_observers[0].getLocalPosition() ,self.m_top_edge_observers[1].getLocalPosition() )
def cutting_edge(self):
return agx.Line( self.m_cutting_edge_observers[0].getLocalPosition(),self.m_cutting_edge_observers[1].getLocalPosition() )