def create_hinge_scene():
size = [0.5, 0.25, 1]
b1, b2 = create_bodies([0, 0, 0], [0, 0, -2.5], size)
demoutils.sim().add(b1)
demoutils.sim().add(b2)
f1 = agx.Frame()
f1.setLocalTranslate(0, 0, size[2])
f1.setLocalRotate(agx.EulerAngles(0, math.radians(90), 0))
hinge1 = agx.Hinge(b1, f1)
demoutils.sim().add(hinge1)
# Make the first motor swing back and forth
speed_controller = AlternatingSpeedController(hinge1.getMotor1D(), 1, 2)
demoutils.sim().add(speed_controller)
distance = (b2.getPosition() - b1.getPosition()).length()
f1 = agx.Frame()
f1.setLocalTranslate(0, 0, -distance / 2)
f1.setLocalRotate(agx.EulerAngles(0, math.radians(90), 0))
f2 = agx.Frame()
f2.setLocalTranslate(0, 0, distance / 2)
f2.setLocalRotate(agx.EulerAngles(0, math.radians(90), 0))
hinge2 = agx.Hinge(b1, f1, b2, f2)
demoutils.sim().add(hinge2)
def LinkBodies_Spring(body1, pos1, body2, pos2, length, KS, KD, visible=False, spring_radius=0.05, spring_turns=50):
spr1 = agx.Frame()
spr1.setTranslate(agxVecify(pos1))
spr2 = agx.Frame()
spr2.setTranslate(agxVecify(pos2))
spring = agx.DistanceJoint(body1, spr1, body2, spr2)
spring.setElasticity(KS)
spring.setDamping(KD)
return spring
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 connect(self, simulation, body1, body2, pos, axis, hingeRange):
assert (body1 and body2)
f1 = agx.Frame()
f2 = agx.Frame()
agx.Constraint.calculateFramesFromWorld(pos, axis, body1, f1, body2,
f2)
hinge = agx.Hinge(body1, f1, body2, f2)
if hingeRange:
hinge.getRange1D().setEnable(True)
hinge.getRange1D().setRange(hingeRange)
simulation.add(hinge)
return hinge
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 create_constraint(**kwds) -> agx.Constraint:
if 'pos' in kwds:
pos = kwds['pos']
else:
pos = agx.Vec3()
if 'axis' in kwds:
axis = kwds['axis']
else:
axis = agx.Vec3.Z_AXIS()
c = kwds['c']
rb1 = kwds['rb1']
rb2 = kwds['rb2']
f1 = agx.Frame()
f2 = agx.Frame()
agx.Constraint.calculateFramesFromBody(pos, axis, rb1, f1, rb2, f2)
return c(rb1, f1, rb2, f2)
def __init__(self):
super().__init__()
len1 = 2
len2 = 1
self.link1 = create_link(len1)
self.link2 = create_link(len2)
self.link2.setPosition(len2, 0, len1)
self.link2.setRotation(agx.EulerAngles(0, math.radians(-90), 0))
self.link1.getGeometries()[0].setEnableCollisions(self.link2.getGeometries()[0], False)
demoutils.create_visual(self.link1)
demoutils.create_visual(self.link2)
self.hinge = demoutils.create_constraint(
pos=agx.Vec3(width, 0, len1),
axis=agx.Vec3(0, 1, 0),
rb1=self.link1,
rb2=self.link2,
c=agx.Hinge) # type: agx.Hinge
self.hinge.setCompliance(1e-8)
self.hinge.getLock1D().setEnable(False)
self.hinge.getMotor1D().setEnable(False)
self.hinge.getRange1D().setEnable(True)
self.hinge.getRange1D().setRange(math.radians(-45), math.radians(75))
f1 = agx.Frame()
f1.setTranslate(agx.Vec3(width, 0, len1 - (len1 * 0.7)))
f2 = agx.Frame()
f2.setTranslate(agx.Vec3(-width, 0, 0))
self.distance = agx.DistanceJoint(self.link1, f1, self.link2, f2)
self.distance.getMotor1D().setEnable(False)
self.distance.getLock1D().setEnable(True)
self.add(self.link1)
self.add(self.link2)
self.add(self.hinge)
self.add(self.distance)
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))
# Create a ground plane for reference
ground = create_body(name="ground",
shape=agxCollide.Box(CYLINDER_LENGTH * 2,
CYLINDER_LENGTH * 2,
GROUND_WIDTH),
position=agx.Vec3(0, 0, 0),
motion_control=agx.RigidBody.STATIC)
sim.add(ground)
rotation_cylinder = agx.OrthoMatrix3x3()
rotation_cylinder.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.Z_AXIS())
material_cylinder = agx.Material("Aluminum")
bulk_material_cylinder = material_cylinder.getBulkMaterial()
bulk_material_cylinder.setPoissonsRatio(ALUMINUM_POISSON_RATIO)
bulk_material_cylinder.setYoungsModulus(ALUMINUM_YOUNG_MODULUS)
# Create cylinders
bottom_cylinder = create_body(
name="bottom_cylinder",
shape=agxCollide.Cylinder(CYLINDER_RADIUS, 3 / 4 * CYLINDER_LENGTH),
position=agx.Vec3(0, 0, GROUND_WIDTH + (3 / 4 * CYLINDER_LENGTH) / 2),
rotation=rotation_cylinder,
motion_control=agx.RigidBody.STATIC,
material=material_cylinder)
sim.add(bottom_cylinder)
middle_cylinder = create_body(
name="middle_cylinder",
shape=agxCollide.Cylinder(CYLINDER_RADIUS - GROOVE_DEPTH,
GROOVE_WIDTH),
position=agx.Vec3(
0, 0, GROUND_WIDTH + (3 / 4 * CYLINDER_LENGTH) + GROOVE_WIDTH / 2),
rotation=rotation_cylinder,
motion_control=agx.RigidBody.STATIC,
material=material_cylinder)
sim.add(middle_cylinder)
top_cylinder = create_body(
name="top_cylinder",
shape=agxCollide.Cylinder(CYLINDER_RADIUS, 1 / 4 * CYLINDER_LENGTH),
position=agx.Vec3(
0, 0, GROUND_WIDTH + (3 / 4 * CYLINDER_LENGTH) + GROOVE_WIDTH +
(1 / 4 * CYLINDER_LENGTH) / 2),
rotation=rotation_cylinder,
motion_control=agx.RigidBody.STATIC,
material=material_cylinder)
sim.add(top_cylinder)
material_ring = agx.Material("Rubber")
bulk_material_ring = material_ring.getBulkMaterial()
bulk_material_ring.setPoissonsRatio(RUBBER_POISSON_RATIO)
bulk_material_ring.setYoungsModulus(RUBBER_YOUNG_MODULUS)
ring = create_ring(name="ring",
radius=RING_RADIUS,
element_shape=agxCollide.Capsule(
RING_CROSS_SECTION, RING_SEGMENT_LENGTH),
num_elements=NUM_RING_ELEMENTS,
constraint_type=agx.LockJoint,
rotation_shift=math.pi / 2,
translation_shift=RING_SEGMENT_LENGTH / 2,
compliance=RING_COMPLIANCE,
center=agx.Vec3(0, 0, CYLINDER_LENGTH + RING_RADIUS),
material=material_ring)
sim.add(ring)
left_ring_element = sim.getRigidBody(LEFT_ELEMENT)
right_ring_element = sim.getRigidBody(RIGHT_ELEMENT)
gripper_left = create_body(
name="gripper_left",
shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER, SIZE_GRIPPER),
position=left_ring_element.getPosition(),
rotation=agx.OrthoMatrix3x3(left_ring_element.getRotation()),
motion_control=agx.RigidBody.DYNAMICS)
sim.add(gripper_left)
gripper_right = create_body(
name="gripper_right",
shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER, SIZE_GRIPPER),
position=right_ring_element.getPosition(),
rotation=agx.OrthoMatrix3x3(right_ring_element.getRotation()),
motion_control=agx.RigidBody.DYNAMICS)
sim.add(gripper_right)
# Disable collisions for grippers
gripper_left_body = sim.getRigidBody("gripper_left")
gripper_left_body.getGeometry("gripper_left").setEnableCollisions(False)
gripper_right_body = sim.getRigidBody("gripper_right")
gripper_right_body.getGeometry("gripper_right").setEnableCollisions(False)
frame_element = agx.Frame()
frame_gripper = agx.Frame()
result = agx.Constraint.calculateFramesFromBody(
agx.Vec3(RING_CROSS_SECTION, 0, 0), agx.Vec3(1, 0, 0),
left_ring_element, frame_element, gripper_left_body, frame_gripper)
print(result)
lock_joint_left = agx.LockJoint(gripper_left_body, frame_gripper,
left_ring_element, frame_element)
lock_joint_left.setName('lock_joint_left')
lock_joint_left.setCompliance(GRIPPER_COMPLIANCE)
sim.add(lock_joint_left)
frame_gripper = agx.Frame()
result = agx.Constraint.calculateFramesFromBody(
agx.Vec3(RING_CROSS_SECTION, 0, 0), agx.Vec3(1, 0, 0),
right_ring_element, frame_element, gripper_right_body, frame_gripper)
print(result)
lock_joint_right = agx.LockJoint(gripper_right_body, frame_gripper,
right_ring_element, frame_element)
lock_joint_right.setName('lock_joint_right')
lock_joint_right.setCompliance(GRIPPER_COMPLIANCE)
sim.add(lock_joint_right)
# Create contact materials
contact_material = sim.getMaterialManager().getOrCreateContactMaterial(
material_cylinder, material_ring)
contact_material.setYoungsModulus(CONTACT_YOUNG_MODULUS)
# Create friction model, DIRECT is more accurate, but slower
# fm = agx.IterativeProjectedConeFriction()
# fm.setSolveType(agx.FrictionModel.DIRECT)
# contact_material.setFrictionModel(fm)
# Create bases for gripper motors
prismatic_base_left = create_hinge_prismatic_base(
"gripper_left",
gripper_left_body,
position_ranges=[(-CYLINDER_RADIUS, CYLINDER_RADIUS),
(-CYLINDER_RADIUS, CYLINDER_RADIUS),
(-(CYLINDER_LENGTH + 2 * RING_RADIUS), RING_RADIUS),
(-math.pi / 4, math.pi / 4)])
sim.add(prismatic_base_left)
prismatic_base_right = create_hinge_prismatic_base(
"gripper_right",
gripper_right_body,
position_ranges=[(-CYLINDER_RADIUS, CYLINDER_RADIUS),
(-CYLINDER_RADIUS, CYLINDER_RADIUS),
(-CYLINDER_LENGTH - RING_RADIUS, RING_RADIUS),
(-math.pi / 4, math.pi / 4)])
sim.add(prismatic_base_right)
# Add keyboard listener
left_motor_x = sim.getConstraint1DOF(
"gripper_left_joint_base_x").getMotor1D()
left_motor_y = sim.getConstraint1DOF(
"gripper_left_joint_base_y").getMotor1D()
left_motor_z = sim.getConstraint1DOF(
"gripper_left_joint_base_z").getMotor1D()
left_motor_rb = sim.getConstraint1DOF("gripper_left_joint_rb").getMotor1D()
right_motor_x = sim.getConstraint1DOF(
"gripper_right_joint_base_x").getMotor1D()
right_motor_y = sim.getConstraint1DOF(
"gripper_right_joint_base_y").getMotor1D()
right_motor_z = sim.getConstraint1DOF(
"gripper_right_joint_base_z").getMotor1D()
right_motor_rb = sim.getConstraint1DOF(
"gripper_right_joint_rb").getMotor1D()
key_motor_map = {
agxSDK.GuiEventListener.KEY_Right: (right_motor_x, 0.1),
agxSDK.GuiEventListener.KEY_Left: (right_motor_x, -0.1),
agxSDK.GuiEventListener.KEY_Up: (right_motor_y, 0.1),
agxSDK.GuiEventListener.KEY_Down: (right_motor_y, -0.1),
65365: (right_motor_z, 0.1),
65366: (right_motor_z, -0.1),
0x64: (left_motor_x, 0.1),
0x61: (left_motor_x, -0.1),
0x32: (left_motor_y,
0.1), # 0x77 W is replaced with 2, due to prior shortcut
0x73: (left_motor_y, -0.1),
0x71: (left_motor_z, 0.1),
0x65: (left_motor_z, -0.1),
0x72: (left_motor_rb, 0.1), # R
0x74: (left_motor_rb, -0.1), # T
0x6f: (right_motor_rb, 0.1), # O
0x70: (right_motor_rb, -0.1)
} # P
sim.add(KeyboardMotorHandler(key_motor_map))
return sim
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 buildScene1(self, app, sim, root):
# Create the Terrain
num_cells_x = 80
num_cells_y = 80
cell_size = 0.15
max_depth = 1.0
agx_heightField = agxCollide.HeightField(num_cells_x, num_cells_y,
(num_cells_x - 1) * cell_size,
(num_cells_y - 1) * cell_size)
# Define the initial height field (random or fixed) depending on if if data collection or test
if self.control_mode == "data_collection":
np_heightField = self.createRandomHeightfield(
num_cells_x, num_cells_y, cell_size)
elif self.control_mode == "mpcc" or self.control_mode == "trajectory_control":
np_heightField = self.createRandomHeightfield(
num_cells_x, num_cells_y, cell_size)
if self.set_height_from_previous:
agx_heightField = self.agx_heightField_previous
else:
agx_heightField = self.setHeightField(agx_heightField,
np_heightField)
terrain = agxTerrain.Terrain.createFromHeightField(
agx_heightField, 5.0)
sim.add(terrain)
# Define Gravity
G = agx.Vec3(0, 0, -10.0)
sim.setUniformGravity(G)
# define the material
terrain.loadLibraryMaterial("sand_1")
terrainMaterial = terrain.getTerrainMaterial()
terrainMaterial.getBulkProperties().setSwellFactor(1.00)
compactionProperties = terrainMaterial.getCompactionProperties()
compactionProperties.setAngleOfReposeCompactionRate(500.0)
terrain.setCompaction(1.05)
# The trenching will reach the bounds of the terrain so we simply remove particles out of bounds
# to get rid of the mateiral in a practical way.
terrain.getProperties().setDeleteSoilParticlesOutsideBounds(True)
if app:
# Setup a renderer for the terrain
renderer = agxOSG.TerrainVoxelRenderer(terrain, root)
renderer.setRenderHeightField(True)
# We choose to render the compaction of the soil to visually denote excavated
# soil from compacted ground
# renderer.setRenderCompaction( True, agx.RangeReal( 1.0, 1.05 ) )
renderer.setRenderHeights(True, agx.RangeReal(-0.4, 0.1))
# renderer.setRenderHeights(True, agx.RangeReal(-0.5,0.5))
renderer.setRenderVoxelSolidMass(False)
renderer.setRenderVoxelFluidMass(False)
renderer.setRenderNodes(False)
renderer.setRenderVoxelBoundingBox(False)
renderer.setRenderSoilParticlesMesh(True)
sim.add(renderer)
# Set contact materials of the terrain and shovel
# This contact material governs the resistance that the shovel will feel when digging into the terrain
# [ Shovel - Terrain ] contact material
shovelMaterial = agx.Material("shovel_material")
terrainMaterial = terrain.getMaterial(
agxTerrain.Terrain.MaterialType_TERRAIN)
shovelTerrainContactMaterial = agx.ContactMaterial(
shovelMaterial, terrainMaterial)
shovelTerrainContactMaterial.setYoungsModulus(1e8)
shovelTerrainContactMaterial.setRestitution(0.0)
shovelTerrainContactMaterial.setFrictionCoefficient(0.4)
sim.add(shovelTerrainContactMaterial)
# Create the trenching shovel body creation, do setup in the Terrain object and
# constrain it to a kinematic that will drive the motion
# Create the bucket rigid body
cuttingEdge, topEdge, forwardVector, bucket = self.createBucket(
default)
sim.add(bucket)
# Create the Shovel object using the previously defined cutting and top edge
shovel = agxTerrain.Shovel(bucket, topEdge, cuttingEdge, forwardVector)
agxUtil.setBodyMaterial(bucket, shovelMaterial)
# Set a margin around the bounding box of the shovel where particles are not to be merged
shovel.setNoMergeExtensionDistance(0.1)
# Add the shovel to the terrain
terrain.add(shovel)
if app: # and self.consecutive_scoop_i < 4:
# Create visual representation of the shovel
node = agxOSG.createVisual(bucket, root)
agxOSG.setDiffuseColor(node, agxRender.Color.Gold())
agxOSG.setAlpha(node, 1.0)
# Set initial bucket rotation
if self.control_mode == "mpcc":
angle_bucket_initial = -0.3 * np.pi
else:
angle_bucket_initial = np.random.uniform(low=-0.25 * np.pi,
high=-0.35 * np.pi)
bucket.setRotation(agx.EulerAngles(0.0, angle_bucket_initial, agx.PI))
# Get the offset of the bucket tip from the COM
tip_offset = shovel.getCuttingEdgeWorld().p2
#
inertia_tensor = bucket.getMassProperties().getInertiaTensor()
mass = bucket.getMassProperties().getMass()
h_offset_sqrd = tip_offset[0]**2 + tip_offset[2]**2
inertia_bucket = inertia_tensor.at(1, 1) + mass * h_offset_sqrd
# Set initial bucket position (for consecutive scoops)
if self.control_mode == "mpcc" or self.control_mode == "trajectory_control":
if self.consecutive_scoop_i == 0:
x_initial_tip = -4.0
elif self.consecutive_scoop_i == 1:
x_initial_tip = -3.6
elif self.consecutive_scoop_i == 2:
x_initial_tip = -3.3
else:
x_initial_tip = -2.6
else:
x_initial_tip = np.random.uniform(low=-4.5, high=-3.0)
# find the soil height at the initial penetration location
hf_grid_initial = terrain.getClosestGridPoint(
agx.Vec3(x_initial_tip, 0.0, 0.0))
height_initial = terrain.getHeight(hf_grid_initial) - 0.05
# Set the initial bucket location such that it is just contacting the soil
position = agx.Vec3(x_initial_tip - tip_offset[0], 0,
height_initial - tip_offset[2])
bucket.setPosition(terrain.getTransform().transformPoint(position))
bucket.setVelocity(0.0, 0.0, 0.0)
# bucket.setAngularVelocity(0.0, 0.05, 0.0)
# Add a lockjoint between a kinematic sphere and the shovel
# in order to have some compliance when moving the shovel
# through the terrain
offset = agx.Vec3(0.0, 0.0, 0.0)
## ADD ALL THE JOINTS TO CONTROL THE BUCKET (x,z,theta)
sphere1 = agx.RigidBody(agxCollide.Geometry(agxCollide.Sphere(.1)))
sphere2 = agx.RigidBody(agxCollide.Geometry(agxCollide.Sphere(.1)))
sphere3 = agx.RigidBody(agxCollide.Geometry(agxCollide.Sphere(.1)))
sphere1.setMotionControl(agx.RigidBody.DYNAMICS)
sphere2.setMotionControl(agx.RigidBody.DYNAMICS)
sphere3.setMotionControl(agx.RigidBody.DYNAMICS)
sphere1.getGeometries()[0].setEnableCollisions(False)
sphere2.getGeometries()[0].setEnableCollisions(False)
sphere3.getGeometries()[0].setEnableCollisions(False)
tip_position = shovel.getCuttingEdgeWorld().p2
tip_position[1] = bucket.getCmPosition()[1]
sphere1.setPosition(tip_position)
sphere2.setPosition(tip_position)
sphere3.setPosition(tip_position)
sphere1.getMassProperties().setMass(0.000001)
sphere2.getMassProperties().setMass(0.000001)
sphere3.getMassProperties().setMass(0.000001)
# print('sphere mass: ', sphere1.getMassProperties().getMass())
sim.add(sphere1)
sim.add(sphere2)
sim.add(sphere3)
# Set prismatic joint for x transalation world - sphere 1
f1 = agx.Frame()
f1.setLocalRotate(agx.EulerAngles(0, math.radians(90), 0))
prismatic1 = agx.Prismatic(sphere1, f1)
# Set prismatic joint for z transalation world - sphere 2
f1 = agx.Frame()
f2 = agx.Frame()
f1.setLocalRotate(agx.EulerAngles(0, math.radians(180), 0))
f2.setLocalRotate(agx.EulerAngles(0, math.radians(180), 0))
prismatic2 = agx.Prismatic(sphere1, f1, sphere2, f2)
# # Set hinge joint for rotation of the bucket
f1 = agx.Frame()
f1.setLocalRotate(agx.EulerAngles(-math.radians(90), 0, 0))
f2 = agx.Frame()
f2.setLocalRotate(agx.EulerAngles(-math.radians(90), 0, 0))
hinge2 = agx.Hinge(sphere2, f1, sphere3, f2)
sim.add(prismatic1)
sim.add(prismatic2)
sim.add(hinge2)
lock = agx.LockJoint(sphere3, bucket)
sim.add(lock)
# Uncomment to lock rotations
# sim.add(agx.LockJoint(sphere2,bucket))
# constant force and torque
operations = [agx.Vec3(0.0, 0.0, 0.0), agx.Vec3(0.0, 0.0, 0.0)]
# Extract soil shape along the bucket excavation direction
x_hf, z_hf = self.extractSoilSurface(terrain, sim)
self.soilShapeEvaluator = SoilSurfaceEvaluator(x_hf, z_hf)
setattr(self.dfl, "soilShapeEvaluator", self.soilShapeEvaluator)
if self.control_mode == "data_collection":
# create driver and add it to the simulation
driver = ForceDriverPID(app, sphere3, lock, hinge2, prismatic1,
prismatic2, terrain, shovel, operations,
self.dt_control)
# Add the current surface evaluator to the controller
setattr(driver, "soilShapeEvaluator", self.soilShapeEvaluator)
# Add the controller to the simulation
sim.add(driver)
elif self.control_mode == "trajectory_control":
# create driver and add it to the simulation
# create driver and add it to the simulation
driver = ForceDriverTrajectory(app, sphere3, lock, hinge2,
prismatic1, prismatic2, terrain,
shovel, operations, self.dt_control)
# Add the current surface evaluator to the controller
setattr(driver, "soilShapeEvaluator", self.soilShapeEvaluator)
setattr(driver, "dfl", self.dfl)
x_path = x_initial_tip + np.array(
[0., 0.5, 1.5, 2.0, 2.5, 3.0, 3.5])
y_soil, _, _, _ = self.soilShapeEvaluator.soil_surf_eval(x_path)
y_path = y_soil + np.array(
[-0.07, -0.25, -0.25, -0.25, -0.25, -0.25, -0.02])
spl_path = spline_path(x_path, y_path)
setattr(driver, "path_eval", spl_path.path_eval)
# Add the controller to the simulation
sim.add(driver)
elif self.control_mode == "mpcc":
# create driver and add it to the simulation
driver = ForceDriverDFL(app, sphere3, lock, hinge2, terrain,
shovel, self.dt_control)
################ MPCC CONTROLLER ############################
# Add the current surface evaluator to the controller
setattr(driver, "soilShapeEvaluator", self.soilShapeEvaluator)
setattr(driver, "dfl", self.dfl)
setattr(driver, "scaling", self.scaling)
x_path = x_initial_tip + np.array([
0.,
0.5,
1.5,
2.0,
2.5,
3.0,
])
y_soil, _, _, _ = self.soilShapeEvaluator.soil_surf_eval(x_path)
y_path = y_soil + np.array(
[-0.07, -0.25, -0.25, -0.25, -0.25, -0.02])
# Set the state constraints
if self.observable_type == "dfl":
x_min = np.array([
x_initial_tip - 0.1, -3., 0.5, -0.5, -2.5, -2.5,
-80000 * self.scaling, -80000 * self.scaling, 0.0,
-70000 * self.scaling, -70000 * self.scaling,
-70000 * self.scaling, -70000 * self.scaling
])
x_max = np.array([
2., 5.0, 2.5, 2.5, 2.5, 2.5, 80000 * self.scaling,
80000 * self.scaling, 3000. * self.scaling,
70000 * self.scaling, 70000 * self.scaling,
70000 * self.scaling, 70000 * self.scaling
])
n_dyn = self.dfl.plant.n
elif self.observable_type == "x":
x_min = np.array(
[x_initial_tip - 0.1, -3., 0.5, -0.5, -2.5, -2.5])
x_max = np.array([2., 5.0, 2.5, 2.5, 2.5, 2.5])
n_dyn = self.dfl.plant.n_x
# Set the input constraints
u_min = np.array([
-100. * self.scaling, -70000 * self.scaling,
-70000 * self.scaling
])
u_max = np.array([
75000. * self.scaling, 70000 * self.scaling,
70000 * self.scaling
])
if self.set_height_from_previous:
pass
else:
self.spl_path = spline_path(x_path, y_path)
# instantiate the MPCC object
mpcc = MPCC(np.zeros((n_dyn, n_dyn)),
np.zeros((n_dyn, self.dfl.plant.n_u)),
x_min,
x_max,
u_min,
u_max,
dt=self.dt_data,
N=50)
# set the observation function, path object and linearization function
setattr(mpcc, "path_eval", self.spl_path.path_eval)
setattr(mpcc, "get_soil_surface",
self.soilShapeEvaluator.soil_surf_eval)
if self.model_has_surface_shape:
print('Linearizing with soil shape')
setattr(mpcc, "get_linearized_model",
self.dfl.linearize_soil_dynamics_koop)
else:
setattr(mpcc, "get_linearized_model",
self.dfl.linearize_soil_dynamics_no_surface)
self.mpcc = copy.copy(mpcc)
pos_tip, vel_tip, acl_tip, ang_tip, omega, alpha = measureBucketState(
sphere3, shovel)
x_i = np.array([
pos_tip[0], pos_tip[2], ang_tip, vel_tip[0], vel_tip[2],
omega[1]
])
eta_i = np.array(
[acl_tip[0], acl_tip[2], alpha[1], 0., 0., 0., 0.])
# Choose the initial path arcposition based on a close initial x tip position
theta_array = np.linspace(-10, 0, num=1000)
for i in range(len(theta_array)):
x_path, y_path = self.spl_path.path_eval(theta_array[i], d=0)
if x_path > pos_tip[0]:
path_initial = theta_array[i]
break
# set initial input (since input cost is differential)
x_0_mpcc = np.concatenate(
(self.dfl.g_Koop(x_i, eta_i, _), np.array([path_initial])))
u_minus_mpcc = np.array([0.0, 0.0, 0.0, 0.0])
driver.last_x_opt = x_0_mpcc
# sets up the new mpcc problem _mpcc
mpcc.setup_new_problem(self.Q_mpcc, self.R_mpcc, self.q_theta_mpcc,
x_0_mpcc, u_minus_mpcc)
setattr(driver, "mpcc", mpcc)
#####################################################################################
# Add the controller to the simulation
sim.add(driver)
# Limit core usage to number of physical cores. Assume that HT/SMT is active
# and divide max threads with 2.
agx.setNumThreads(0)
n = int(agx.getNumThreads() / 2 - 1)
agx.setNumThreads(n)
# Setup initial camera view
if app:
createHelpText(sim, app)
return terrain, shovel, driver, sphere3
def build_simulation(goal=False):
"""Builds simulations for both start and goal configurations
:param bool goal: toggles between simulation definition of start and goal configurations
:return agxSDK.Simulation: simulation object
"""
assembly_name = "start_"
goal_string = ""
if goal:
assembly_name = "goal_"
goal_string = "_goal"
# 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))
# Create a new empty Assembly
scene = agxSDK.Assembly()
scene.setName(assembly_name + "assembly")
# Add start assembly to simulation
sim.add(scene)
# Create a ground plane for reference
if not goal:
ground = create_body(name="ground", shape=agxCollide.Box(LENGTH, LENGTH, GROUND_WIDTH),
position=agx.Vec3(LENGTH / 2, 0, -(GROUND_WIDTH + SIZE_GRIPPER / 2 + LENGTH)),
motion_control=agx.RigidBody.STATIC)
scene.add(ground)
# Create cable
cable = agxCable.Cable(RADIUS, RESOLUTION)
cable.setName("DLO" + goal_string)
gripper_left = create_body(name="gripper_left" + goal_string,
shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER, SIZE_GRIPPER),
position=agx.Vec3(0, 0, 0), motion_control=agx.RigidBody.DYNAMICS)
scene.add(gripper_left)
gripper_right = create_body(name="gripper_right" + goal_string,
shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER, SIZE_GRIPPER),
position=agx.Vec3(LENGTH, 0, 0),
motion_control=agx.RigidBody.DYNAMICS)
scene.add(gripper_right)
# Disable collisions for grippers
gripper_left_body = scene.getRigidBody("gripper_left" + goal_string)
gripper_left_body.getGeometry("gripper_left" + goal_string).setEnableCollisions(False)
gripper_right_body = scene.getRigidBody("gripper_right" + goal_string)
gripper_right_body.getGeometry("gripper_right" + goal_string).setEnableCollisions(False)
logger.info("Mass of grippers: {}".format(scene.getRigidBody("gripper_right" + goal_string).calculateMass()))
# Create Frames for each gripper:
# Cables are attached passing through the attachment point along the Z axis of the body's coordinate frame.
# The translation specified in the transformation is relative to the body and not the world
left_transform = agx.AffineMatrix4x4()
left_transform.setTranslate(SIZE_GRIPPER + RADIUS, 0, 0)
left_transform.setRotate(agx.Vec3.Z_AXIS(), agx.Vec3.Y_AXIS()) # Rotation matrix which switches Z with Y
frame_left = agx.Frame(left_transform)
right_transform = agx.AffineMatrix4x4()
right_transform.setTranslate(- SIZE_GRIPPER - RADIUS, 0, 0)
right_transform.setRotate(agx.Vec3.Z_AXIS(), -agx.Vec3.Y_AXIS()) # Rotation matrix which switches Z with -Y
frame_right = agx.Frame(right_transform)
cable.add(agxCable.FreeNode(agx.Vec3(SIZE_GRIPPER + RADIUS, 0, 0))) # Fix cable to gripper_left
cable.add(agxCable.FreeNode(agx.Vec3(LENGTH - SIZE_GRIPPER - RADIUS, 0, 0))) # Fix cable to gripper_right
# Try to initialize cable
report = cable.tryInitialize()
if report.successful():
logger.debug("Successful cable initialization.")
else:
logger.error(report.getActualError())
actual_length = report.getLength()
logger.info("Actual length: " + str(actual_length))
# Add cable plasticity
plasticity = agxCable.CablePlasticity()
plasticity.setYieldPoint(YIELD_POINT, agxCable.BEND) # set torque required for permanent deformation
cable.addComponent(plasticity) # NOTE: Stretch direction is always elastic
# Define material
material = agx.Material("Aluminum")
bulk_material = material.getBulkMaterial()
bulk_material.setPoissonsRatio(POISSON_RATIO)
bulk_material.setYoungsModulus(YOUNG_MODULUS)
cable.setMaterial(material)
# Add cable to simulation
scene.add(cable)
# Add segment names and get first and last segment
count = 1
iterator = cable.begin()
segment_left = iterator.getRigidBody()
segment_left.setName('dlo_' + str(count) + goal_string)
while not iterator.isEnd():
count += 1
segment_right = iterator.getRigidBody()
segment_right.setName('dlo_' + str(count) + goal_string)
iterator.inc()
# Add hinge constraints
hinge_joint_left = agx.Hinge(scene.getRigidBody("gripper_left" + goal_string), frame_left, segment_left)
hinge_joint_left.setName('hinge_joint_left' + goal_string)
motor_left = hinge_joint_left.getMotor1D()
motor_left.setEnable(True)
motor_left_param = motor_left.getRegularizationParameters()
motor_left_param.setCompliance(1e12)
motor_left.setLockedAtZeroSpeed(False)
lock_left = hinge_joint_left.getLock1D()
lock_left.setEnable(False)
# Set range of hinge joint
# range_left = hinge_joint_left.getRange1D()
# range_left.setEnable(True)
# range_left.setRange(agx.RangeReal(-math.pi / 2, math.pi / 2))
scene.add(hinge_joint_left)
hinge_joint_right = agx.Hinge(scene.getRigidBody("gripper_right" + goal_string), frame_right, segment_right)
hinge_joint_right.setName('hinge_joint_right' + goal_string)
motor_right = hinge_joint_right.getMotor1D()
motor_right.setEnable(True)
motor_right_param = motor_right.getRegularizationParameters()
motor_right_param.setCompliance(1e12)
motor_right.setLockedAtZeroSpeed(False)
lock_right = hinge_joint_right.getLock1D()
lock_right.setEnable(False)
# Set range of hinge joint
# range_right = hinge_joint_right.getRange1D()
# range_right.setEnable(True)
# range_right.setRange(agx.RangeReal(-math.pi / 2, math.pi / 2))
scene.add(hinge_joint_right)
# Create base for gripper motors
prismatic_base_right = create_locked_prismatic_base("gripper_right" + goal_string, gripper_right_body, compliance=0,
motor_ranges=[(-FORCE_RANGE, FORCE_RANGE),
(-FORCE_RANGE, FORCE_RANGE),
(-FORCE_RANGE, FORCE_RANGE)],
position_ranges=[
(-LENGTH + 2 * (2 * RADIUS + SIZE_GRIPPER),
0),
(-LENGTH, LENGTH),
(-LENGTH, LENGTH)],
compute_forces=True,
lock_status=[False, False, False])
scene.add(prismatic_base_right)
prismatic_base_left = create_locked_prismatic_base("gripper_left" + goal_string, gripper_left_body, compliance=0,
lock_status=[True, True, True])
scene.add(prismatic_base_left)
return sim
def build_simulation(goal=False):
"""Builds simulations for both start and goal configurations
:param bool goal: toggles between simulation definition of start and goal configurations
:return agxSDK.Simulation: simulation object
"""
assembly_name = "start_"
goal_string = ""
if goal:
assembly_name = "goal_"
goal_string = "_goal"
# 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))
# Create a new empty Assembly
scene = agxSDK.Assembly()
scene.setName(assembly_name + "assembly")
# Add start assembly to simulation
sim.add(scene)
# Create a ground plane for reference and obstacle
if not goal:
ground = create_body(name="ground",
shape=agxCollide.Box(LENGTH, LENGTH,
GROUND_WIDTH),
position=agx.Vec3(
0, 0,
-(GROUND_WIDTH + SIZE_GRIPPER / 2 + LENGTH)),
motion_control=agx.RigidBody.STATIC)
sim.add(ground)
# Create two grippers
gripper_left = create_body(name="gripper_left" + goal_string,
shape=agxCollide.Box(SIZE_GRIPPER, SIZE_GRIPPER,
SIZE_GRIPPER),
position=agx.Vec3(-LENGTH / 2, 0, 0),
motion_control=agx.RigidBody.DYNAMICS)
scene.add(gripper_left)
gripper_right = create_body(name="gripper_right" + goal_string,
shape=agxCollide.Box(SIZE_GRIPPER,
SIZE_GRIPPER,
SIZE_GRIPPER),
position=agx.Vec3(LENGTH / 2, 0, 0),
motion_control=agx.RigidBody.DYNAMICS)
scene.add(gripper_right)
gripper_left_body = gripper_left.getRigidBody("gripper_left" + goal_string)
gripper_right_body = gripper_right.getRigidBody("gripper_right" +
goal_string)
# Create material
material_cylinder = agx.Material("cylinder_material")
bulk_material_cylinder = material_cylinder.getBulkMaterial()
bulk_material_cylinder.setPoissonsRatio(POISSON_RATIO)
bulk_material_cylinder.setYoungsModulus(YOUNG_MODULUS)
cylinder = create_body(name="obstacle" + goal_string,
shape=agxCollide.Cylinder(CYLINDER_RADIUS,
CYLINDER_LENGTH),
position=agx.Vec3(0, 0, -2 * CYLINDER_RADIUS),
motion_control=agx.RigidBody.STATIC,
material=material_cylinder)
scene.add(cylinder)
# Create cable
cable = agxCable.Cable(RADIUS, RESOLUTION)
# Create Frames for each gripper:
# Cables are attached passing through the attachment point along the Z axis of the body's coordinate frame.
# The translation specified in the transformation is relative to the body and not the world
left_transform = agx.AffineMatrix4x4()
left_transform.setTranslate(SIZE_GRIPPER + RADIUS, 0, 0)
left_transform.setRotate(
agx.Vec3.Z_AXIS(),
agx.Vec3.Y_AXIS()) # Rotation matrix which switches Z with Y
frame_left = agx.Frame(left_transform)
right_transform = agx.AffineMatrix4x4()
right_transform.setTranslate(-SIZE_GRIPPER - RADIUS, 0, 0)
right_transform.setRotate(
agx.Vec3.Z_AXIS(),
-agx.Vec3.Y_AXIS()) # Rotation matrix which switches Z with -Y
frame_right = agx.Frame(right_transform)
cable.add(
agxCable.FreeNode(agx.Vec3(-LENGTH / 2 + SIZE_GRIPPER + RADIUS, 0,
0))) # Fix cable to gripper_left
cable.add(
agxCable.FreeNode(agx.Vec3(LENGTH / 2 - SIZE_GRIPPER - RADIUS, 0,
0))) # Fix cable to gripper_right
# Set cable name and properties
cable.setName("DLO" + goal_string)
properties = cable.getCableProperties()
properties.setYoungsModulus(YOUNG_MODULUS, agxCable.BEND)
properties.setYoungsModulus(YOUNG_MODULUS, agxCable.TWIST)
properties.setYoungsModulus(YOUNG_MODULUS, agxCable.STRETCH)
material_wire = cable.getMaterial()
wire_material = material_wire.getBulkMaterial()
wire_material.setPoissonsRatio(POISSON_RATIO)
wire_material.setYoungsModulus(YOUNG_MODULUS)
cable.setMaterial(material_wire)
# Add cable plasticity
plasticity = agxCable.CablePlasticity()
plasticity.setYieldPoint(
YIELD_POINT,
agxCable.BEND) # set torque required for permanent deformation
plasticity.setYieldPoint(
YIELD_POINT,
agxCable.STRETCH) # set torque required for permanent deformation
cable.addComponent(plasticity) # NOTE: Stretch direction is always elastic
# Tell MaterialManager to create and return a contact material which will be used
# when two geometries both with this material is in contact
contact_material = sim.getMaterialManager().getOrCreateContactMaterial(
material_cylinder, material_wire)
contact_material.setYoungsModulus(CONTACT_YOUNG_MODULUS)
# Create a Friction model, which we tell the solver to solve ITERATIVELY (faster)
fm = agx.IterativeProjectedConeFriction()
fm.setSolveType(agx.FrictionModel.DIRECT)
contact_material.setFrictionModel(fm)
# Try to initialize cable
report = cable.tryInitialize()
if report.successful():
logger.debug("Successful cable initialization.")
else:
logger.error(report.getActualError())
# Add cable to simulation
sim.add(cable)
# Add segment names and get first and last segment
segment_count = 0
iterator = cable.begin()
segment_left = iterator.getRigidBody()
segment_left.setName('dlo_' + str(segment_count + 1) + goal_string)
segment_right = None
while not iterator.isEnd():
segment_count += 1
segment_right = iterator.getRigidBody()
segment_right.setName('dlo_' + str(segment_count + 1) + goal_string)
iterator.inc()
# Add hinge constraints
hinge_joint_left = agx.Hinge(
sim.getRigidBody("gripper_left" + goal_string), frame_left,
segment_left)
hinge_joint_left.setName('hinge_joint_left' + goal_string)
motor_left = hinge_joint_left.getMotor1D()
motor_left.setEnable(False)
motor_left_param = motor_left.getRegularizationParameters()
motor_left_param.setCompliance(1e12)
motor_left.setLockedAtZeroSpeed(False)
lock_left = hinge_joint_left.getLock1D()
lock_left.setEnable(False)
range_left = hinge_joint_left.getRange1D()
range_left.setEnable(True)
range_left.setRange(agx.RangeReal(-math.pi / 2, math.pi / 2))
sim.add(hinge_joint_left)
hinge_joint_right = agx.Hinge(
sim.getRigidBody("gripper_right" + goal_string), frame_right,
segment_right)
hinge_joint_right.setName('hinge_joint_right' + goal_string)
motor_right = hinge_joint_right.getMotor1D()
motor_right.setEnable(False)
motor_right_param = motor_right.getRegularizationParameters()
motor_right_param.setCompliance(1e12)
motor_right.setLockedAtZeroSpeed(False)
lock_right = hinge_joint_right.getLock1D()
lock_right.setEnable(False)
range_right = hinge_joint_right.getRange1D()
range_right.setEnable(True)
range_right.setRange(agx.RangeReal(-math.pi / 2, math.pi / 2))
sim.add(hinge_joint_right)
# Create bases for gripper motors
prismatic_base_left = create_locked_prismatic_base(
"gripper_left" + goal_string,
gripper_left_body,
compliance=0,
motor_ranges=[(-FORCE_RANGE, FORCE_RANGE), (-FORCE_RANGE, FORCE_RANGE),
(-FORCE_RANGE, FORCE_RANGE)],
position_ranges=[(-LENGTH / 2 + CYLINDER_RADIUS,
LENGTH / 2 - CYLINDER_RADIUS),
(-CYLINDER_LENGTH / 3, CYLINDER_LENGTH / 3),
(-(GROUND_WIDTH + SIZE_GRIPPER / 2 + LENGTH), 0)],
lock_status=[False, False, False])
sim.add(prismatic_base_left)
prismatic_base_right = create_locked_prismatic_base(
"gripper_right" + goal_string,
gripper_right_body,
compliance=0,
motor_ranges=[(-FORCE_RANGE, FORCE_RANGE), (-FORCE_RANGE, FORCE_RANGE),
(-FORCE_RANGE, FORCE_RANGE)],
position_ranges=[(-LENGTH / 2 + CYLINDER_RADIUS,
LENGTH / 2 - CYLINDER_RADIUS),
(-CYLINDER_LENGTH / 3, CYLINDER_LENGTH / 3),
(-(GROUND_WIDTH + SIZE_GRIPPER / 2 + LENGTH), 0)],
lock_status=[False, False, False])
sim.add(prismatic_base_right)
return sim
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 a ground plane
ground = create_body(name="ground",
shape=agxCollide.Box(GROUND_LENGTH_X, GROUND_LENGTH_Y,
GROUND_HEIGHT),
position=agx.Vec3(0, 0, -0.005),
motion_control=agx.RigidBody.STATIC)
sim.add(ground)
# Creates poles
for i, position in enumerate(POLE_POSITION_OFFSETS):
create_pole(id=i, sim=sim, position=position, material=material_hard)
# Create gripper
gripper = create_body(name="gripper",
shape=agxCollide.Sphere(0.002),
position=agx.Vec3(0.0, 0.0,
GRIPPER_HEIGHT + DIAMETER / 2.0),
motion_control=agx.RigidBody.DYNAMICS,
material=material_hard)
gripper.getRigidBody("gripper").getGeometry("gripper").setEnableCollisions(
False)
sim.add(gripper)
# Create base for pusher motors
prismatic_base = create_locked_prismatic_base(
"gripper",
gripper.getRigidBody("gripper"),
position_ranges=[(-GRIPPER_MAX_X, GRIPPER_MAX_X),
(-GRIPPER_MAX_Y, GRIPPER_MAX_Y),
(GRIPPER_MIN_Z, GRIPPER_MAX_Z)],
motor_ranges=[(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE)])
sim.add(prismatic_base)
# Create rope and set name + properties
rubber_band = agxCable.Cable(RADIUS, RESOLUTION)
rubber_band.setName("DLO")
material_rubber_band = rubber_band.getMaterial()
rubber_band_material = material_rubber_band.getBulkMaterial()
rubber_band_material.setPoissonsRatio(PEG_POISSON_RATIO)
properties = rubber_band.getCableProperties()
properties.setYoungsModulus(YOUNG_MODULUS_BEND, agxCable.BEND)
properties.setYoungsModulus(YOUNG_MODULUS_TWIST, agxCable.TWIST)
properties.setYoungsModulus(YOUNG_MODULUS_STRETCH, agxCable.STRETCH)
# Initialize dlo on circle
steps = DLO_CIRCLE_STEPS
for a in np.linspace(-np.pi / 2, (3.0 / 2.0) * np.pi - 2 * np.pi / steps,
steps):
x = np.cos(a) * DIAMETER / 2.0
z = np.sin(a) * DIAMETER / 2.0
rubber_band.add(agxCable.FreeNode(x, 0, GRIPPER_HEIGHT + z))
sim.add(rubber_band)
segments_cable = list()
cable = agxCable.Cable.find(sim, "DLO")
segment_iterator = cable.begin()
n_segments = cable.getNumSegments()
for i in range(n_segments):
if not segment_iterator.isEnd():
seg = segment_iterator.getRigidBody()
seg.setAngularVelocityDamping(1e4)
mass_props = seg.getMassProperties()
mass_props.setMass(1.25 * mass_props.getMass())
segments_cable.append(seg)
segment_iterator.inc()
# Get segments at ends and middle
s0 = segments_cable[0]
s1 = segments_cable[int(n_segments / 2)]
s2 = segments_cable[-1]
# Add ball joint between gripper and rubber band
f0 = agx.Frame()
f1 = agx.Frame()
ball_joint = agx.BallJoint(gripper.getRigidBody("gripper"), f0, s1, f1)
sim.add(ball_joint)
# Connect ends of rubber band
f0 = agx.Frame()
f0.setLocalTranslate(0.0, 0.0,
-1 * np.pi * DIAMETER / cable.getNumSegments())
f1 = agx.Frame()
lock = agx.LockJoint(s0, f0, s2, f1)
lock.setCompliance(1.0e-4)
sim.add(lock)
# Try to initialize dlo
report = rubber_band.tryInitialize()
if report.successful():
print("Successful dlo initialization.")
else:
print(report.getActualError())
# Add rope to simulation
sim.add(rubber_band)
# Set rope material
material_rubber_band = rubber_band.getMaterial()
material_rubber_band.setName("rope_material")
contactMaterial = sim.getMaterialManager().getOrCreateContactMaterial(
material_hard, material_rubber_band)
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()
key_motor_map = {
agxSDK.GuiEventListener.KEY_Up: (motor_y, 0.2),
agxSDK.GuiEventListener.KEY_Down: (motor_y, -0.2),
agxSDK.GuiEventListener.KEY_Right: (motor_x, 0.2),
agxSDK.GuiEventListener.KEY_Left: (motor_x, -0.2),
65365: (motor_z, 0.2),
65366: (motor_z, -0.2)
}
sim.add(KeyboardMotorHandler(key_motor_map))
rbs = rubber_band.getRigidBodies()
for i in range(len(rbs)):
rbs[i].setName('dlo_' + str(i + 1))
return sim
