def create_distribution_table(shape_paths, sizes, weights, material):
dt = agx.EmitterDistributionTable()
n = len(shape_paths) * len(sizes)
for shape_path in shape_paths:
# Get the original size of the model
rb_original = create_rigid_body_from_obj(shape_path, agx.Matrix3x3(),
None)
size_original = inertia_tensor_size_estimation(
rb_original.getMassProperties())
# add a correctly scaled rb to the distribution table for each size
for s, w in zip(sizes, weights):
scale = agx.Matrix3x3() * s / size_original
rb = create_rigid_body_from_obj(shape_path, scale, material)
model = agx.RigidBodyEmitterDistributionModel(rb, w / n)
dt.addModel(model)
return dt
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 load_shape(path) -> agxCollide.Trimesh:
r"""Load shape from path and create a trimesh
:return: Trimesh of shape"""
return createTrimeshFromFile(path,
agxCollide.Trimesh.REMOVE_DUPLICATE_VERTICES,
agx.Matrix3x3())
def load_shape(path) -> agxCollide.Trimesh:
return createTrimeshFromFile(
path, agxCollide.Trimesh.REMOVE_DUPLICATE_VERTICES, agx.Matrix3x3())
def create_gripper_peg_in_hole(sim=None,
name="",
material=None,
position=agx.Vec3(0.0, 0.0, 0.0),
geometry_transform=agx.AffineMatrix4x4(),
geometry_scaling=agx.Matrix3x3(agx.Vec3(0.045)),
joint_ranges=None,
force_ranges=None):
# Create gripper object
gripper_mesh = agxUtil.createTrimeshFromWavefrontOBJ(
MESH_GRIPPER_FILE, agxCollide.Trimesh.NO_WARNINGS, geometry_scaling)
gripper_geom = agxCollide.Geometry(
gripper_mesh,
agx.AffineMatrix4x4.rotate(agx.Vec3(0, 1, 0), agx.Vec3(0, 0, 1)))
gripper_body = agx.RigidBody(name + "_body")
gripper_body.add(gripper_geom)
gripper_body.setMotionControl(agx.RigidBody.DYNAMICS)
gripper_body.setPosition(position)
gripper_body.setCmRotation(agx.EulerAngles(0.0, np.pi, 0.0))
if material is not None:
gripper_geom.setMaterial(material)
sim.add(gripper_body)
# Add kinematic structure
rotation_y_to_z = agx.OrthoMatrix3x3()
rotation_y_to_z.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.Z_AXIS())
rotation_y_to_x = agx.OrthoMatrix3x3()
rotation_y_to_x.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.X_AXIS())
base_z = create_body(name=name + "_base_z",
shape=agxCollide.Cylinder(0.005, 0.025),
position=position,
rotation=rotation_y_to_z,
motion_control=agx.RigidBody.DYNAMICS,
disable_collisions=True)
sim.add(base_z)
base_y = create_body(name=name + "_base_y",
shape=agxCollide.Cylinder(0.005, 0.025),
position=position,
motion_control=agx.RigidBody.DYNAMICS,
disable_collisions=True)
sim.add(base_y)
base_x = create_body(name=name + "_base_x",
shape=agxCollide.Cylinder(0.005, 0.025),
position=position,
rotation=rotation_y_to_x,
motion_control=agx.RigidBody.DYNAMICS,
disable_collisions=True)
sim.add(base_x)
base_x_body = base_x.getRigidBody("gripper_base_x")
base_y_body = base_y.getRigidBody("gripper_base_y")
base_z_body = base_z.getRigidBody("gripper_base_z")
base_x_body.getGeometry("gripper_base_x").setEnableCollisions(False)
base_y_body.getGeometry("gripper_base_y").setEnableCollisions(False)
base_z_body.getGeometry("gripper_base_z").setEnableCollisions(False)
# Add prismatic joints between bases
joint_base_x = agx.Prismatic(agx.Vec3(1, 0, 0), base_x_body)
joint_base_x.setEnableComputeForces(True)
joint_base_x.setEnable(True)
joint_base_x.setName(name + "_joint_base_x")
sim.add(joint_base_x)
joint_base_y = agx.Prismatic(agx.Vec3(0, 1, 0), base_x_body, base_y_body)
joint_base_y.setEnableComputeForces(True)
joint_base_y.setEnable(True)
joint_base_y.setName(name + "_joint_base_y")
sim.add(joint_base_y)
joint_base_z = agx.Prismatic(agx.Vec3(0, 0, -1), base_y_body, base_z_body)
joint_base_z.setEnableComputeForces(True)
joint_base_z.setEnable(True)
joint_base_z.setName(name + "_joint_base_z")
sim.add(joint_base_z)
# Hinge joint to rotate gripper around y axis
hf = agx.HingeFrame()
hf.setCenter(position)
hf.setAxis(agx.Vec3(0, 1, 0))
joint_rot_y = agx.Hinge(hf, base_z_body, gripper_body)
joint_rot_y.setEnableComputeForces(True)
joint_rot_y.setName(name + "_joint_rot_y")
sim.add(joint_rot_y)
# Set joint ranges
if joint_ranges is not None:
# x range
joint_base_x_range_controller = joint_base_x.getRange1D()
joint_base_x_range_controller.setRange(joint_ranges["t_x"][0],
joint_ranges["t_x"][1])
joint_base_x_range_controller.setEnable(True)
# y range
joint_base_y_range_controller = joint_base_y.getRange1D()
joint_base_y_range_controller.setRange(joint_ranges["t_y"][0],
joint_ranges["t_y"][1])
joint_base_y_range_controller.setEnable(True)
# z range
joint_base_z_range_controller = joint_base_z.getRange1D()
joint_base_z_range_controller.setRange(joint_ranges["t_z"][0],
joint_ranges["t_z"][1])
joint_base_z_range_controller.setEnable(True)
# rot y
joint_rot_y_range_controller = joint_rot_y.getRange1D()
joint_rot_y_range_controller.setRange(joint_ranges["r_y"][0],
joint_ranges["r_y"][1])
joint_rot_y_range_controller.setEnable(True)
# Enable motors
joint_base_x_motor = joint_base_x.getMotor1D()
joint_base_x_motor.setEnable(True)
joint_base_x_motor.setLockedAtZeroSpeed(False)
joint_base_y_motor = joint_base_y.getMotor1D()
joint_base_y_motor.setEnable(True)
joint_base_y_motor.setLockedAtZeroSpeed(False)
joint_base_z_motor = joint_base_z.getMotor1D()
joint_base_z_motor.setEnable(True)
joint_base_z_motor.setLockedAtZeroSpeed(False)
joint_rot_y_motor = joint_rot_y.getMotor1D()
joint_rot_y_motor.setEnable(True)
joint_rot_y_motor.setLockedAtZeroSpeed(False)
# Set max forces in motors
if force_ranges is not None:
# force x
joint_base_x_motor.setForceRange(force_ranges['t_x'][0],
force_ranges['t_x'][1])
# force y
joint_base_y_motor.setForceRange(force_ranges['t_y'][0],
force_ranges['t_y'][1])
# force z
joint_base_z_motor.setForceRange(force_ranges['t_z'][0],
force_ranges['t_z'][1])
# force rotation around y
joint_rot_y_motor.setForceRange(force_ranges['r_y'][0],
force_ranges['r_y'][1])
def build_simulation():
# Instantiate a simulation
sim = agxSDK.Simulation()
# By default the gravity vector is 0,0,-9.81 with a uniform gravity field. (we CAN change that
# too by creating an agx.PointGravityField for example).
# AGX uses a right-hand coordinate system (That is Z defines UP. X is right, and Y is into the screen)
if not GRAVITY:
logger.info("Gravity off.")
g = agx.Vec3(0, 0, 0) # remove gravity
sim.setUniformGravity(g)
# Get current delta-t (timestep) that is used in the simulation?
dt = sim.getTimeStep()
logger.debug("default dt = {}".format(dt))
# Change the timestep
sim.setTimeStep(TIMESTEP)
# Confirm timestep changed
dt = sim.getTimeStep()
logger.debug("new dt = {}".format(dt))
# Define materials
material_hard = agx.Material("Aluminum")
material_hard_bulk = material_hard.getBulkMaterial()
material_hard_bulk.setPoissonsRatio(ALUMINUM_POISSON_RATIO)
material_hard_bulk.setYoungsModulus(ALUMINUM_YOUNG_MODULUS)
# Create gripper
create_gripper_peg_in_hole(sim=sim,
name="gripper",
material=material_hard,
position=agx.Vec3(0.0, 0.0, 0.35),
geometry_transform=agx.AffineMatrix4x4(),
joint_ranges=JOINT_RANGES,
force_ranges=FORCE_RANGES)
# Create hollow cylinde with hole
scaling_cylinder = agx.Matrix3x3(agx.Vec3(0.0275))
hullMesh = agxUtil.createTrimeshFromWavefrontOBJ(MESH_HOLLOW_CYLINDER_FILE,
0, scaling_cylinder)
hullGeom = agxCollide.Geometry(
hullMesh,
agx.AffineMatrix4x4.rotate(agx.Vec3(0, 1, 0), agx.Vec3(0, 0, 1)))
hollow_cylinder = agx.RigidBody("hollow_cylinder")
hollow_cylinder.add(hullGeom)
hollow_cylinder.setMotionControl(agx.RigidBody.STATIC)
hullGeom.setMaterial(material_hard)
sim.add(hollow_cylinder)
# Create rope and set name + properties
peg = agxCable.Cable(RADIUS, RESOLUTION)
peg.setName("DLO")
material_peg = peg.getMaterial()
peg_material = material_peg.getBulkMaterial()
peg_material.setPoissonsRatio(PEG_POISSON_RATIO)
properties = peg.getCableProperties()
properties.setYoungsModulus(YOUNG_MODULUS_BEND, agxCable.BEND)
properties.setYoungsModulus(YOUNG_MODULUS_TWIST, agxCable.TWIST)
properties.setYoungsModulus(YOUNG_MODULUS_STRETCH, agxCable.STRETCH)
# Add connection between cable and gripper
tf_0 = agx.AffineMatrix4x4()
tf_0.setTranslate(0.0, 0, 0.075)
peg.add(agxCable.BodyFixedNode(sim.getRigidBody("gripper_body"), tf_0))
peg.add(agxCable.FreeNode(0.0, 0.0, 0.1))
sim.add(peg)
segment_iterator = peg.begin()
n_segments = peg.getNumSegments()
for i in range(n_segments):
if not segment_iterator.isEnd():
seg = segment_iterator.getRigidBody()
seg.setAngularVelocityDamping(1e3)
segment_iterator.inc()
# Try to initialize rope
report = peg.tryInitialize()
if report.successful():
print("Successful rope initialization.")
else:
print(report.getActualError())
# Add rope to simulation
sim.add(peg)
# Set rope material
material_peg = peg.getMaterial()
material_peg.setName("rope_material")
contactMaterial = sim.getMaterialManager().getOrCreateContactMaterial(
material_hard, material_peg)
contactMaterial.setYoungsModulus(1e12)
fm = agx.IterativeProjectedConeFriction()
fm.setSolveType(agx.FrictionModel.DIRECT)
contactMaterial.setFrictionModel(fm)
# Add keyboard listener
motor_x = sim.getConstraint1DOF("gripper_joint_base_x").getMotor1D()
motor_y = sim.getConstraint1DOF("gripper_joint_base_y").getMotor1D()
motor_z = sim.getConstraint1DOF("gripper_joint_base_z").getMotor1D()
motor_rot_y = sim.getConstraint1DOF("gripper_joint_rot_y").getMotor1D()
key_motor_map = {
agxSDK.GuiEventListener.KEY_Up: (motor_y, 0.5),
agxSDK.GuiEventListener.KEY_Down: (motor_y, -0.5),
agxSDK.GuiEventListener.KEY_Right: (motor_x, 0.5),
agxSDK.GuiEventListener.KEY_Left: (motor_x, -0.5),
65365: (motor_z, 0.5),
65366: (motor_z, -0.5),
120: (motor_rot_y, 5),
121: (motor_rot_y, -5)
}
sim.add(KeyboardMotorHandler(key_motor_map))
rbs = peg.getRigidBodies()
for i in range(len(rbs)):
rbs[i].setName('dlo_' + str(i + 1))
return sim