def create_shovel(**kwargs) -> TerrainTool:
shovel = TerrainTool(size=[(agx.Vec3(0.01, 1.5, 0.2), 10),
(agx.Vec3(0.01, 1.5, 0.4), -20)],
create_walls=False)
simulation().add(shovel.body)
create_visual(shovel)
return shovel
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 __init__(self,
n_substeps=1,
reward_type="dense",
observation_type="state",
headless=False,
image_size=[64, 64],
**kwargs):
"""Initializes a PegInHoleEnv object
:param args: arguments for agxViewer.
:param scene_path: path to binary file in assets/ folder containing serialized simulation defined in sim/ folder
:param n_substeps: number os simulation steps per call to step().
:param end_effectors: list of EndEffector objects, defining controllable constraints.
:param observation_config: ObservationConfig object, defining the types of observations.
:param camera_config: dictionary containing EYE, CENTER, UP information for rendering, with lighting info.
:param reward_config: reward configuration object, defines success condition and reward function.
"""
self.reward_type = reward_type
self.segment_pos_old = None
self.n_segments = None
self.headless = headless
camera_distance = 0.1 # meters
camera_config = CameraConfig(eye=agx.Vec3(0, -1.0, 0.2),
center=agx.Vec3(0, 0, 0.2),
up=agx.Vec3(0., 0., 1.0),
light_position=agx.Vec4(
0, -camera_distance, camera_distance,
1.),
light_direction=agx.Vec3(0., 0., -1.))
if 'agxViewer' in kwargs:
args = sys.argv + kwargs['agxViewer']
else:
args = sys.argv
# Change window size
args.extend(["--window", "400", "600"])
no_graphics = headless and observation_type not in ("rgb", "depth",
"rgb_and_depth")
# Disable rendering in headless mode
if headless:
args.extend(["--osgWindow", "0"])
if headless and observation_type == "gt":
# args.extend(["--osgWindow", "0"])
args.extend(["--agxOnly", "1", "--osgWindow", "0"])
super(PegInHoleEnv,
self).__init__(scene_path=SCENE_PATH,
n_substeps=n_substeps,
observation_type=observation_type,
n_actions=4,
camera_pose=camera_config.camera_pose,
no_graphics=no_graphics,
image_size=image_size,
args=args)
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 setupCamera(self, app):
cameraData = app.getCameraData()
cameraData.eye = agx.Vec3(-3., -10., 2.4)
cameraData.center = agx.Vec3(-3., 0., 0.)
cameraData.up = agx.Vec3(0.0, 0.0, 0.0)
cameraData.nearClippingPlane = 10
cameraData.farClippingPlane = 5000
app.applyCameraData(cameraData)
def setup_camera(app):
""" Setup the default camera angle etc. """
camera_data = app.getCameraData()
camera_data.eye = agx.Vec3(0.5, 0, 0.25)
camera_data.center = agx.Vec3(0, 0, 0)
camera_data.up = agx.Vec3(0, 0, 1)
camera_data.nearClippingPlane = 0.1
camera_data.farClippingPlane = 5000
app.applyCameraData(camera_data)
def agxVecify(vec, transform = True):
if transform:
vec = xyzTransform(vec)
if type(vec) == type([]):
agxVec = agx.Vec3(float(vec[0]), float(vec[1]), float(vec[2]))
elif type(vec) == type(np.array([])):
agxVec = agx.Vec3(float(vec[0]), float(vec[1]), float(vec[2]))
else:
agxVec = agx.Vec3(vec)
return agxVec
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 createConstraint(**kwargs):
"""Create constraint given type, rb1, local/world axis and local/world position.
Examples:
hinge = createConstraint( type = agx.Hinge,
rb1 = firstRigidBody,
rb2 = otherRigidBody,
axis = axisRelRb1,
point = pointRelRb1 )
Arguments:
type: agx.Constraint -- Constraint type.
rb1: agx.RigidBody -- First rigid body (not None)
rb2: agx.RigidBody -- Second rigid body (world if not given or None)
axis: agx.Vec3 -- Axis given in rb1 frame (either localAxis or worldAxis must be given).
worldAxis: agx.Vec3 -- Axis given in world frame (either localAxis or worldAxis must be given).
point: agx.Vec3 -- Point given in rb1 frame (either localPoint or worldPoint must be given).
worldPoint: agx.Vec3 -- Point given in world frame (either localPoint or worldPoint must be given).
position: agx.Vec3 -- See point.
worldPosition: agx.Vec3 -- See worldPoint.
Returns:
[agx.Constraint] -- Constraint of given type if successful - otherwise None.
"""
rb1 = kwargs['rb1']
if rb1 is None:
print('Unable to create constraint - rb1 not given or None')
return None
rb2 = kwargs.get('rb2', None)
worldAxis = kwargs.get('worldAxis', agx.Vec3.Z_AXIS())
worldPoint = kwargs.get(
'worldPoint', agx.Vec3()) if 'worldPoint' in kwargs else kwargs.get(
'worldPosition', agx.Vec3())
if 'axis' in kwargs:
worldAxis = rb1.getFrame().transformVectorToWorld(kwargs['axis'])
if 'point' in kwargs:
worldPoint = rb1.getFrame().transformPointToWorld(kwargs['point'])
elif 'position' in kwargs:
worldPoint = rb1.getFrame().transformPointToWorld(kwargs['position'])
f1 = agx.Frame()
f2 = agx.Frame()
if not agx.Constraint.calculateFramesFromWorld(worldPoint, worldAxis, rb1,
f1, rb2, f2):
print(
'Unable to create constraint - calculateFramesFromWorld failed to initialize constraint frames.'
)
return None
return kwargs['type'](rb1, f1, rb2, f2)
def update_propulsion(self):
local_forward = agx.Vec3(1, 0, 0)
rotation = self.getRotation()
world_forward = rotation * local_forward
force = world_forward * self.m_propulsion_force
force = force * -1
right_force = agx.Vec3()
left_force = agx.Vec3()
if self.m_turn == 0:
right_force = force
left_force = force
self.m_body.addForceAtLocalPosition(left_force, self.m_left_propeller)
self.m_body.addForceAtLocalPosition(right_force, self.m_right_propeller)
def build_scene():
sim = demoutils.sim()
# 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, Color.Green())
sim.add(floor_geometry) # Add the geometry to the simulation
sim.add(create_box(agx.Vec3(-2, 3, 5)))
sim.add(create_sphere(agx.Vec3(0, 1, 5)))
sim.add(create_capsule(agx.Vec3(3, 5, 5)))
sim.add(create_cylinder(agx.Vec3(-3, -2, 5)))
def connect(rb1: agx.RigidBody, rb2: agx.RigidBody):
axis = agx.Vec3(0, 0, -1)
pos = agx.Vec3(0.0, 0.007, 0)
hinge = snakeapp.create_constraint(
pos=pos, axis=axis, c=agx.Hinge, rb1=rb1, rb2=rb2) # type: agx.Hinge
hinge.setCompliance(1E-12)
hinge.getMotor1D().setEnable(True)
hinge.getMotor1D().setCompliance(1E-12)
hinge.getLock1D().setEnable(False)
hinge.getRange1D().setEnable(True)
hinge.getRange1D().setRange(-math.pi / 2, math.pi)
self.add(hinge)
self.servos.append(hinge)
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 keyboard(self, key, alt, x, y, down):
handled = False
if key == agxSDK.GuiEventListener.KEY_Left:
self.ball.setPosition(self.ball.getPosition() + agx.Vec3(-self.interval, 0, 0))
handled = True
elif key == agxSDK.GuiEventListener.KEY_Right:
self.ball.setPosition(self.ball.getPosition() + agx.Vec3(self.interval, 0, 0))
handled = True
elif key == agxSDK.GuiEventListener.KEY_Up:
self.ball.setPosition(self.ball.getPosition() + agx.Vec3(0, self.interval, 0))
handled = True
elif key == agxSDK.GuiEventListener.KEY_Down:
self.ball.setPosition(self.ball.getPosition() + agx.Vec3(0, -self.interval, 0))
handled = True
return handled
def PreSetup(args, SetupFunction):
if agxPython.getContext():
return
init = agx.AutoInit()
## Create an application with graphics etc.
app = agxOSG.ExampleApplication()
dec = app.getSceneDecorator()
dec.setBackgroundColor(agx.Vec3(1,1,1))
dec.setLogoFile('textures/TF-loader.png')
dec.setLogoLocation(agxOSG.SceneDecorator.FREE)
width = 0.25
dec.setLogoPosition(0.5-width/2, 0.3)
dec.setMaximumLogoDimension(width, 1.0)
## Create a command line parser. sys.executable will point to python executable
## in this case, because getArgumentName(0) needs to match the C argv[0] which
## is the name of the program running
argParser = agxIO.ArgumentParser([sys.executable] + args)
app.addScene(argParser.getArgumentName(1), "buildScene", ord('1'), True)
## Call the init method of ExampleApplication
## It will setup the viewer, windows etc.
if app.init(argParser):
app.run()
else:
print("An error occurred while initializing ExampleApplication.")
def keyboard(self, key, x, y, alt, keydown):
if keydown and key == self.hopper:
if(self.body.getVelocity().z() < 1E-4):
self.body.setVelocity(self.body.getVelocity()+agx.Vec3(0,0,5))
else:
return False
return True
def __init__(self, shape: agxCollide.Shape):
self.instance = shape
self.type = shape.getType() if shape else -1
self.type_name = shape.getTypeName() if shape else 'None'
self.center = shape.getCenter() if shape else agx.Vec3()
if shape is None:
pass
elif shape.asBox():
self.half_extents = shape.asBox().getHalfExtents()
elif shape.asCapsule():
self.radius = shape.asCapsule().getRadius()
self.height = shape.asCapsule().getHeight()
elif shape.asCylinder():
self.radius = shape.asCylinder().getRadius()
self.height = shape.asCylinder().getHeight()
elif shape.asPlane():
self.normal = shape.asPlane().getNormal()
self.distance = shape.asPlane().getDistance()
elif shape.asSphere():
self.radius = shape.asSphere().getRadius()
elif shape.asMesh():
self.num_triangles = shape.asMesh().getNumTriangles()
self.num_vertices = shape.asMesh().getNumVertices()
self.is_valid_and_closed = shape.asMesh().isValidAndClosed()
self.has_half_edge = shape.asMesh().hasHalfEdge()
def __init__(self, app, lockSphere, lock, hinge, prismatic_x, prismatic_z,
terrain, shovel, operations, dt_control):
super(ForceDriverPID, self).__init__()
self.lockSphere = lockSphere
self.lock = lock
self.hinge = hinge
self.prismatic_x = prismatic_x
self.prismatic_z = prismatic_z
self.bucket = shovel.getRigidBody()
self.shovel = shovel
self.terrain = terrain
self.operations = operations
self.forceLimit = 5e4
self.dt_control = dt_control
lock.setEnableComputeForces(True)
self.theta_d = -0.2
self.ang_d = 1.55
self.v_d = 0.2
self.soil_force_last = agx.Vec3(0., 0., 0.)
self.force = self.operations[0]
self.torque = 0.0
self.t_last_control = -100.
self.t_last_setpoint = -100.
self.integ_e_x = 0.0
self.integ_e_z = 0.0
self.integ_e_omega = 0.0
self.integ_e_ang = 0.0
def reset(self):
logger.info("reset")
did_reset_sim = False
while not did_reset_sim:
did_reset_sim = self._reset_sim()
# Randomly initialize cylinder position
goal_pos_new = np.random.uniform([-0.1, -0.1], [0.1, -0.025])
self.sim.getRigidBody("obstacle_goal").setPosition(
agx.Vec3(goal_pos_new[0], goal_pos_new[1], 0.005))
n_inital_random = 20
for k in range(n_inital_random):
if k == 0 or not k % 5:
action = self.action_space.sample()
self._set_action(action)
self.sim.stepForward()
# Wait several steps after initalization
n_inital_wait = 10
for k in range(n_inital_wait):
self.sim.stepForward()
self.segment_pos_old = self._compute_segments_pos()
obs = self._get_observation()
return obs
def build_scene(rosid=10, visual=True):
""" Creates the scene to simulate. """
brick_types = [duplo_bricks.BrickTypes.SPECIAL8X2PLUS2X2, \
duplo_bricks.BrickTypes.STANDARD2X2, \
duplo_bricks.BrickTypes.FAST4X2]
x_pos = [-0.05, 0.0, 0.05, 0.1, 0, 0.05, -0.05, 0, 0.05]
y_pos = [-0.1, -0.1, -0.1, -0.1, -0.05, -0.05, 0, 0, 0]
z_pos = [0.000001] * 9
start_positions = []
#Goal positions, unbalanced
#x_pos = [0.0, 0.0, 0.05, 0.05, 0, 0.05,-0.05, 0, 0.05 ]
#y_pos = [0.0, 0.0, -0.1, -0.1, -0.05, -0.05, 0, 0, 0]
#z_pos = [0.0192+0.002, 0.0, 0.000001, 0.0, -0.05, -0.05, 0, 0, 0]
#Goal positions, with red
#x_pos = [0.0, 0.0, 0.0, 0.05, 0, 0.05,-0.05, 0, 0.05 ]
#y_pos = [0.0, -0.1, 0.0, -0.1, -0.05, -0.05, 0, 0, 0]
#z_pos = [0.0192, 0.0, 0.000001, 0.0, -0.05, -0.05, 0, 0, 0]
for i in range(3):
start_positions.append(agx.Vec3(x_pos[i], y_pos[i], z_pos[i]))
scene_settings = common_functions.SceneSettings()
scene_settings.brick_types = brick_types
scene_settings.start_positions = start_positions
scene_settings.timestep = 1 / 100
scene_settings.visual = visual
scene_settings.listener_pool = common_functions.ListenerPool(size=24)
return common_functions.main_loop(scene_settings,
rosid=rosid,
ros_step=0.1)
def create_box(position: agx.Vec3, scale):
shape = agxCollide.Box(agx.Vec3(0.5 * scale, 0.5 * scale, 0.5 * scale))
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
oneLegRobotApp.create_visual(body)
return body
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 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 __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 updateCamera(self):
cameraData = self.app.getCameraData()
cameraData.eye = self.position
cameraData.center = self.looker
cameraData.up = agx.Vec3(0, 0, 1)
cameraData.nearClippingPlane = 0.1
cameraData.farClippingPlane = 5000
self.app.applyCameraData(cameraData)
def set_center_obstacle(sim, center_pos):
ground = sim.getRigidBody("ground")
ground.setPosition(agx.Vec3(center_pos[0], center_pos[1], -0.005))
for i in range(3):
sim.getRigidBody("cylinder_low_" + str(i)).setPosition(
agx.Vec3(center_pos[0] + POLE_POSITION_OFFSETS[i][0],
center_pos[1] + POLE_POSITION_OFFSETS[i][1], 0.0))
sim.getRigidBody("cylinder_inner_" + str(i)).setPosition(
agx.Vec3(center_pos[0] + POLE_POSITION_OFFSETS[i][0],
center_pos[1] + POLE_POSITION_OFFSETS[i][1], 0.005))
sim.getRigidBody("cylinder_top_" + str(i)).setPosition(
agx.Vec3(center_pos[0] + POLE_POSITION_OFFSETS[i][0],
center_pos[1] + POLE_POSITION_OFFSETS[i][1], 0.01))
def _set_obstacle_center(self, center_pos):
ground = self.sim.getRigidBody("ground")
ground.setPosition(agx.Vec3(center_pos[0], center_pos[1], -0.005))
for i in range(3):
self.sim.getRigidBody("cylinder_low_" + str(i)).setPosition(
agx.Vec3(center_pos[0] + POLE_OFFSET[i][0],
center_pos[1] + POLE_OFFSET[i][1], 0.0))
self.sim.getRigidBody("cylinder_inner_" + str(i)).setPosition(
agx.Vec3(center_pos[0] + POLE_OFFSET[i][0],
center_pos[1] + POLE_OFFSET[i][1], 0.005))
self.sim.getRigidBody("cylinder_top_" + str(i)).setPosition(
agx.Vec3(center_pos[0] + POLE_OFFSET[i][0],
center_pos[1] + POLE_OFFSET[i][1], 0.01))
def post(self, t):
eye = agxVecify([0,8,0])
pos = agxVecify([0,0,0])
self.rti_color.setViewMatrixAsLookAt(eye, pos, agx.Vec3(0,1,0))
filename_color = "Second_cam.png"
self.rti_color.saveImage(filename_color)
def build_lock_joint(part1, part2, pos1, pos2) -> agx.LockJoint:
"""
creates a agx lock joint between two parts and returns the resoult
Args:
part1:
part2:
pos1: The lock possition on part 2
pos2: The lock possition on part 2
Returns:
a lock joint
"""
f1 = agx.Frame()
f2 = agx.Frame()
f1.setTranslate(agx.Vec3(*pos1))
f2.setTranslate(agx.Vec3(*pos2))
return demoutils.create_constraint(
pos1=f1, pos2=f2, rb1=part1, rb2=part2,
c=agx.LockJoint) # type: agx.LockJoint
def add_slope():
half_extents = agx.Vec3(length, 0.1, 0.005)
slope = agxCollide.Geometry(agxCollide.Box(half_extents),
agx.AffineMatrix4x4.translate(half_extents.x(), 0, -half_extents.z()))
slope.setRotation(agx.EulerAngles(0, -self._slope_angle, 0))
snakeapp.create_visual(slope, diffuse_color=agxRender.Color.Red())
slope.setMaterial(self.material)
self.add(slope)
return slope
