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 create_capsule(position: agx.Vec3):
shape = agxCollide.Capsule(0.5, 1)
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
demoutils.create_visual(body)
body.setRotation(agx.EulerAngles(agx.PI_2, 0, 0))
return body
def create_capsule(position: agx.Vec3, scale):
shape = agxCollide.Capsule(0.5 * scale, 1 * scale)
geometry = agxCollide.Geometry(shape)
body = agx.RigidBody(geometry)
body.setPosition(position)
oneLegRobotApp.create_visual(body)
body.setRotation(agx.EulerAngles(agx.PI_2, 0, 0))
return body
def create_fenders(self, fender_material, r_fender, half_width, half_height, half_length):
fender_color = agxRender.Color.Black()
fender = agxCollide.Geometry(agxCollide.Capsule(0.2, half_width * 2))
fender.setPosition(half_length, 0, 0)
self.m_body.add(fender)
agxOSG.setDiffuseColor(agxOSG.createVisual(fender, demoutils.root()), fender_color)
fender.setMaterial(fender_material)
def create_fenders(fender_material, half_width, half_length):
fender = agxCollide.Geometry(agxCollide.Capsule(0.8, half_width * 2))
fender.setPosition(half_length, 0, 0.2)
fender.setMaterial(fender_material)
return fender
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)