def create_cylinder(position: agx.Vec3):
shape = agxCollide.Cylinder(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_cylinder(position: agx.Vec3, scale):
shape = agxCollide.Cylinder(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 add_cylinderShape(MiroSystem, radius, height, density, pos, texture='test.jpg', scale=[1,1], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, color=[0.5, 0.5, 0.5]):
'''system, size_x, size_y, size_z, pos, texture, scale = [5,5], hitbox = True/False'''
# Convert position to chrono vector, supports using chvector as input as well
agxSim = agxPython.getContext().environment.getSimulation()
agxApp = agxPython.getContext().environment.getApplication()
agxRoot = agxPython.getContext().environment.getSceneRoot()
agxPos = agxVecify(pos)
agxRotAxis = agxVecify(rotAxis)
scale = scaleLimit(scale)
# Create a cylinder
body_geo = agxCollide.Geometry(agxCollide.Cylinder(radius, height))
body_geo.setName("body")
body_geo.setEnableCollisions(Collide)
body_cylinder = agx.RigidBody(body_geo)
body_cylinder.getMassProperties().setMass(body_geo.calculateVolume()*density)
if Fixed:
body_cylinder.setMotionControl(1)
body_cylinder.setPosition(agxPos)
rotateBody(body_cylinder, rotX=-90)
rotateBody(body_cylinder, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees)
# Collision shape
# if(Collide):
# Visualization shape
body_shape = agxOSG.createVisual(body_cylinder, agxRoot)
# Body texture
if texture:
# Filter 'textures/' out of the texture name, it's added later
if len(texture) > len('textures/'):
if texture[0:len('textures/')] == 'textures/':
texture = texture[len('textures/'):]
if TEXTURES_ON:
if texture not in LOADED_TEXTURES.keys():
agxTex = agxOSG.createTexture(TEXTURE_PATH+texture)
LOADED_TEXTURES.update({texture: agxTex})
agxOSG.setTexture(body_shape, LOADED_TEXTURES[texture], True, agxOSG.DIFFUSE_TEXTURE, -scale[0], scale[1])
else:
color = backupColor(texture, color)
texture = False
if not texture:
agxColor = agxRender.Color(color[0], color[1], color[2])
agxOSG.setDiffuseColor(body_shape, agxColor)
if len(color) > 3:
agxOSG.setAlpha(body_shape, color[3])
agxSim.add(body_cylinder)
return body_cylinder
def create_pole(id, sim, position, material):
x = position[0]
y = position[1]
# Lower part
rotation_cylinder = agx.OrthoMatrix3x3()
rotation_cylinder.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.Z_AXIS())
cylinder = create_body(name="cylinder_low_" + str(id),
shape=agxCollide.Cylinder(POLE_RADIUS, 0.005),
position=agx.Vec3(x, y, 0.0),
rotation=rotation_cylinder,
motion_control=agx.RigidBody.KINEMATICS,
material=material)
sim.add(cylinder)
# Middle part
rotation_cylinder = agx.OrthoMatrix3x3()
rotation_cylinder.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.Z_AXIS())
cylinder = create_body(name="cylinder_inner_" + str(id),
shape=agxCollide.Cylinder(POLE_RADIUS - 0.0007,
0.005),
position=agx.Vec3(x, y, 0.005),
rotation=rotation_cylinder,
motion_control=agx.RigidBody.KINEMATICS,
material=material)
sim.add(cylinder)
# Upper part
rotation_cylinder = agx.OrthoMatrix3x3()
rotation_cylinder.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.Z_AXIS())
cylinder = create_body(name="cylinder_top_" + str(id),
shape=agxCollide.Cylinder(POLE_RADIUS, 0.005),
position=agx.Vec3(x, y, 0.01),
rotation=rotation_cylinder,
motion_control=agx.RigidBody.KINEMATICS,
material=material)
sim.add(cylinder)
def add_stud(self, props, material, x, y, z=0):
""" Adds stud at (x, y) """
base = agxCollide.Geometry(agxCollide.Cylinder(props.stud_radius, props.studcyl_height))
base.setMaterial(material)
rotate_x(base, 90)
top = agxCollide.Geometry(agxCollide.Cone(props.studtop_inner_rad, props.stud_radius, props.studtop_height))
top.setMaterial(material)
rotate_x(top, 90)
self.brick_rb.add(base)
self.brick_rb.add(top)
base.setLocalPosition(x, y, z + props.height + props.studcyl_height / 2)
top.setLocalPosition(x, y, z + props.height + props.studcyl_height)
def addcylinder(sim, root, dims, pos, Fixed=True, color = agxRender.Color.Red(), texture=False):
if type(pos) == type([]):
pos = agx.Vec3(pos[0], pos[1], pos[2])
cyl = agx.RigidBody( agxCollide.Geometry( agxCollide.Cylinder(dims[0], dims[1])))
cyl.setPosition(pos)
if(Fixed):
cyl.setMotionControl(1)
sim.add(cyl)
vis_body = agxOSG.createVisual(cyl, root)
if texture:
agxOSG.setTexture(vis_body, texture, True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0)
else:
agxOSG.setDiffuseColor(vis_body, color)
return cyl
def createBeam(self, pos, length, rotY):
agxSim = agxPython.getContext().environment.getSimulation()
agxApp = agxPython.getContext().environment.getApplication()
agxRoot = agxPython.getContext().environment.getSceneRoot()
agxPos = agxVecify(pos)
self.laser_geo = agxCollide.Geometry(agxCollide.Cylinder(0.004, length))
self.laser_geo.setName("body")
# self.laser_geo.setEnableCollisions(False)
self.laser_body = agx.RigidBody(self.laser_geo)
self.laser_body.setMotionControl(1)
self.laser_body.setPosition(agxPos)
rotateBody(self.laser_body, rotY=rotY, rotDegrees=False)
# Visualization shape
self.laser_vis = agxOSG.createVisual(self.laser_body, agxRoot)
agxOSG.setAlpha(self.laser_vis, 0.5)
agxSim.add(self.laser_body)
agxSim.add(self)
def cylinders(half_length, half_height) -> agxCollide.Geometry:
length = 2 * half_length
radius = half_height * 1.2
return agxCollide.Geometry(
agxCollide.Cylinder(radius, length - 2 * radius))
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 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(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 buildBot(sim, root, bot_pos, controller='Arrows', drivetrain = 'FWD', color=False):
body_wid = 0.32
body_len = 0.6
body_hei = 0.16
wheel_rad = 0.07
wheel_wid = 0.02
wheel_dmp = -0.02
body = agx.RigidBody( agxCollide.Geometry( agxCollide.Box(body_wid/2, body_len/2, body_hei/2)))
body.setPosition(bot_pos[0], bot_pos[1], bot_pos[2] + body_hei/2 + wheel_rad + wheel_dmp )
# body.setMotionControl(1)
body.setRotation(agx.Quat(np.pi, agx.Vec3(0,0,1)))
sim.add(body)
vis_body = agxOSG.createVisual(body, root)
if color:
agxOSG.setDiffuseColor(vis_body, color)
else:
agxOSG.setTexture(vis_body, 'flames.png', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0)
wheelLF = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(wheel_rad, wheel_wid)))
wheelLF.setPosition(bot_pos[0]-(body_wid/2+wheel_wid/2), bot_pos[1]+(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad)
# wheelLF.setMotionControl(1)
wheelLF.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(wheelLF)
# agxOSG.setDiffuseColor(agxOSG.createVisual(wheelLF, root), agxRender.Color.Red())
wheelRF = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(wheel_rad, wheel_wid)))
wheelRF.setPosition(bot_pos[0]+(body_wid/2+wheel_wid/2), bot_pos[1]+(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad)
# wheelRF.setMotionControl(1)
wheelRF.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(wheelRF)
# agxOSG.setDiffuseColor(agxOSG.createVisual(wheelRF, root), agxRender.Color.Red())
wheelLB = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(wheel_rad, wheel_wid)))
wheelLB.setPosition(bot_pos[0]-(body_wid/2+wheel_wid/2), bot_pos[1]-(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad)
# wheelLB.setMotionControl(1)
wheelLB.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(wheelLB)
# agxOSG.setDiffuseColor(agxOSG.createVisual(wheelLB, root), agxRender.Color.Red())
wheelRB = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(wheel_rad, wheel_wid)))
wheelRB.setPosition(bot_pos[0]+(body_wid/2+wheel_wid/2), bot_pos[1]-(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad)
# wheelRB.setMotionControl(1)
wheelRB.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(wheelRB)
# agxOSG.setDiffuseColor(agxOSG.createVisual(wheelRB, root), agxRender.Color.Red())
for wheel in [wheelLB, wheelLF, wheelRB, wheelRF]:
vis_body = agxOSG.createVisual(wheel, root)
agxOSG.setTexture(vis_body, 'tire.png', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0)
light_rad = 0.02
light_dep = 0.01
headlightL = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(light_rad, light_dep)))
headlightL.setPosition( bot_pos[0] + 0.79*body_wid/2, bot_pos[1] + body_len/2+light_dep/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp)
# headlightL.setMotionControl(1)
# headlightL.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(headlightL)
agxOSG.setTexture(agxOSG.createVisual(headlightL, root), 'light.png', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,1,0))
hf.setCenter(agx.Vec3( bot_pos[0] + 0.79*body_wid/2, bot_pos[1] + body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp ))
HLL = agx.Hinge(hf, body, headlightL)
sim.add(HLL)
headlightR = agx.RigidBody(agxCollide.Geometry( agxCollide.Cylinder(light_rad, light_dep)))
headlightR.setPosition(bot_pos[0] -0.79*body_wid/2, bot_pos[1] + body_len/2+light_dep/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp )
# headlightR.setMotionControl(1)
# headlightL.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(headlightR)
agxOSG.setTexture(agxOSG.createVisual(headlightR, root), 'light.png', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,1,0))
hf.setCenter(agx.Vec3(bot_pos[0] -0.79*body_wid/2, bot_pos[1] + body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp ))
HLR = agx.Hinge(hf, body, headlightR)
sim.add(HLR)
light_wid = 0.012
light_hei = 0.02
light_dep = 0.003
taillightL = agx.RigidBody(agxCollide.Geometry( agxCollide.Box(light_wid, light_dep, light_hei)))
taillightL.setPosition(bot_pos[0] -0.79*body_wid/2,bot_pos[1] -(body_len/2+light_dep/2), bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp)
# taillightL.setMotionControl(1)
# headlightL.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(taillightL)
agxOSG.setDiffuseColor(agxOSG.createVisual(taillightL, root), agxRender.Color.Red())
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,1,0))
hf.setCenter(agx.Vec3(bot_pos[0] -0.79*body_wid/2, bot_pos[1] -body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp + light_hei/3))
TLL_hi = agx.Hinge(hf, body, taillightL)
sim.add(TLL_hi)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,1,0))
hf.setCenter(agx.Vec3(bot_pos[0] -0.79*body_wid/2, bot_pos[1] -body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp - light_hei/3))
TLL_low = agx.Hinge(hf, body, taillightL)
sim.add(TLL_low)
taillightR = agx.RigidBody(agxCollide.Geometry( agxCollide.Box(light_wid, light_dep, light_hei)))
taillightR.setPosition( bot_pos[0] + 0.79*body_wid/2,bot_pos[1] -(body_len/2+light_dep/2), bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp)
# taillightR.setMotionControl(1)
# headlightL.setRotation(agx.Quat(np.pi/2, agx.Vec3(0,0,1)))
sim.add(taillightR)
agxOSG.setDiffuseColor(agxOSG.createVisual(taillightR, root), agxRender.Color.Red())
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,1,0))
hf.setCenter(agx.Vec3( bot_pos[0] + 0.79*body_wid/2,bot_pos[1]-body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp + light_hei/3))
TLR = agx.Hinge(hf, body, taillightR)
sim.add(TLR)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,1,0))
hf.setCenter(agx.Vec3( bot_pos[0] + 0.79*body_wid/2,bot_pos[1]-body_len/2, bot_pos[2] + 0.7*body_hei + wheel_rad + wheel_dmp - light_hei/3))
TLR = agx.Hinge(hf, body, taillightR)
sim.add(TLR)
windangle = np.pi/4
windshield = agx.RigidBody( agxCollide.Geometry( agxCollide.Box(0.9*body_wid/2, 0.005, body_hei/3)))
windshield.setPosition(bot_pos[0], bot_pos[1]+body_len/5, bot_pos[2] + body_hei + wheel_rad + wheel_dmp + np.cos(windangle)*body_hei/3)
# windshield.setTorque(0,0,100)
# windshield.setMotionControl(2)
windshield.setRotation(agx.Quat(windangle, agx.Vec3(1,0,0)))
sim.add(windshield)
agxOSG.setTexture(agxOSG.createVisual(windshield, root), 'windshield.jpg', True, agxOSG.DIFFUSE_TEXTURE, 1.0, 1.0)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,0,1))
hf.setCenter(agx.Vec3(-body_wid/3, body_len/5, bot_pos[2] + body_hei + wheel_rad + wheel_dmp))
windh1 = agx.Hinge(hf, body, windshield)
sim.add(windh1)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(0,0,1))
hf.setCenter(agx.Vec3(body_wid/3, body_len/5, bot_pos[2] + body_hei + wheel_rad + wheel_dmp))
windh2 = agx.Hinge(hf, body, windshield)
sim.add(windh2)
x_ax = agx.Vec3(1,0,0)
y_ax = agx.Vec3(0,1,0)
z_ax = agx.Vec3(0,0,1)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(-1,0,0))
hf.setCenter(agx.Vec3(bot_pos[0]-body_wid/2, bot_pos[1]+(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad))
axleLF = agx.Hinge(hf, body, wheelLF)
sim.add(axleLF)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(-1,0,0))
hf.setCenter(agx.Vec3(bot_pos[0]-body_wid/2, bot_pos[1]-(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad))
axleLB = agx.Hinge(hf, body, wheelLB)
sim.add(axleLB)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3( 1,0,0))
hf.setCenter(agx.Vec3(bot_pos[0]+body_wid/2, bot_pos[1]-(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad))
axleRB = agx.Hinge(hf, body, wheelRB)
sim.add(axleRB)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3( 1,0,0))
hf.setCenter(agx.Vec3(bot_pos[0]+body_wid/2, bot_pos[1]+(body_len/2-wheel_rad*1.8), bot_pos[2]+wheel_rad))
axleRF = agx.Hinge(hf, body, wheelRF)
sim.add(axleRF)
wheels = [wheelLF, wheelRF] # default FWD
if drivetrain == 'FWD':
wheels = [wheelLF, wheelRF]
elif drivetrain == 'RWD':
wheels = [wheelLB, wheelRB]
elif drivetrain == 'AWD':
wheels = [wheelLF, wheelRF, wheelLB, wheelRB]
if controller == 'Numpad':
sim.add(WheelControllerNumpad(wheels))
sim.add(Fjoink(body).setHopper(agxSDK.GuiEventListener.KEY_KP_Subtract))
else:
sim.add(WheelControllerArrows(wheels))
sim.add(Fjoink(body))
# return a pointer to the body
return body
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 box with pocket
side_length = 0.15
thickness_outer_wall = 0.01
body = create_body(name="ground", shape=agxCollide.Box(side_length, side_length, 0.01),
position=agx.Vec3(0, 0, -0.005),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(body)
body = create_body(name="walls", shape=agxCollide.Box(thickness_outer_wall, side_length, 0.04),
position=agx.Vec3(side_length + thickness_outer_wall, 0, 0.0),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(body)
body = create_body(name="walls", shape=agxCollide.Box(thickness_outer_wall, side_length, 0.04),
position=agx.Vec3(-(side_length + thickness_outer_wall), 0, 0.0),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(body)
body = create_body(name="walls",
shape=agxCollide.Box(side_length + 2 * thickness_outer_wall, thickness_outer_wall, 0.04),
position=agx.Vec3(0, -(side_length + thickness_outer_wall), 0.0),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(body)
body = create_body(name="walls",
shape=agxCollide.Box(side_length + 2 * thickness_outer_wall, thickness_outer_wall, 0.04),
position=agx.Vec3(0, side_length + thickness_outer_wall, 0.0),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(body)
# Create gripper 0
gripper_0 = create_body(name="gripper_0",
shape=agxCollide.Sphere(0.005),
position=agx.Vec3(-(LENGTH/2), OFFSET_Y, 0.0025),
motion_control=agx.RigidBody.DYNAMICS,
material=material_hard)
gripper_0.getRigidBody("gripper_0").getGeometry("gripper_0").setEnableCollisions(False)
sim.add(gripper_0)
# Create base for gripper motors
prismatic_base_0 = create_locked_prismatic_base("gripper_0", gripper_0.getRigidBody("gripper_0"),
position_ranges=[(-side_length*2, side_length*2),
(-side_length*2, side_length*2),
(-0.1, 0.01)],
motor_ranges=[(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE)],
lock_status=[False, False, False])
sim.add(prismatic_base_0)
# Create gripper
gripper_1 = create_body(name="gripper_1",
shape=agxCollide.Sphere(0.005),
position=agx.Vec3((LENGTH/2), OFFSET_Y, 0.0025),
motion_control=agx.RigidBody.DYNAMICS,
material=material_hard)
gripper_1.getRigidBody("gripper_1").getGeometry("gripper_1").setEnableCollisions(False)
sim.add(gripper_1)
# Create base for gripper motors
prismatic_base_0 = create_locked_prismatic_base("gripper_1", gripper_1.getRigidBody("gripper_1"),
position_ranges=[(-side_length*2, side_length*2),
(-side_length*2, side_length*2),
(-0.1, 0.01)],
motor_ranges=[(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE)],
lock_status=[False, False, False])
sim.add(prismatic_base_0)
# Create goal obstacle
goal_obstacle = create_body(name="obstacle_goal",
shape=agxCollide.Cylinder(2 * R_GOAL_OBSTACLE, 0.1),
position=agx.Vec3(0.0,0.-0.075, 0.005),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(goal_obstacle)
rotation_cylinder = agx.OrthoMatrix3x3()
rotation_cylinder.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.Z_AXIS())
goal_obstacle.setRotation(rotation_cylinder)
# Create obstacles
obs_pos = OBSTACLE_POSITIONS
for i in range(0, len(obs_pos)):
obstacle = create_body(name="obstacle",
shape=agxCollide.Box(0.01, 0.015, 0.05),
position=agx.Vec3(obs_pos[i][0], obs_pos[i][1], 0.005),
motion_control=agx.RigidBody.STATIC,
material=material_hard)
sim.add(obstacle)
# Create rope and set name + properties
dlo = agxCable.Cable(RADIUS, RESOLUTION)
dlo.setName("DLO")
material_rubber_band= dlo.getMaterial()
rubber_band_material = material_rubber_band.getBulkMaterial()
rubber_band_material.setPoissonsRatio(PEG_POISSON_RATIO)
properties = dlo.getCableProperties()
properties.setYoungsModulus(YOUNG_MODULUS_BEND, agxCable.BEND)
properties.setYoungsModulus(YOUNG_MODULUS_TWIST, agxCable.TWIST)
properties.setYoungsModulus(YOUNG_MODULUS_STRETCH, agxCable.STRETCH)
dlo.add(agxCable.FreeNode(gripper_0.getRigidBody("gripper_0").getPosition()))
dlo.add(agxCable.FreeNode(gripper_1.getRigidBody("gripper_1").getPosition()))
#
# hf = agx.HingeFrame()
# hf.setCenter(gripper_0.getRigidBody("gripper_0").getPosition())
# hf.setAxis(agx.Vec3(0,1,0))
# hinge_0 = agx.Hinge(hf, base_z, rot_y)
# agx.Hinge()
# dlo.add(agxCable.BodyFixedNode(gripper_0.getRigidBody("gripper_0"), agx.Vec3()))
# dlo.add(agxCable.BodyFixedNode(gripper_1.getRigidBody("gripper_1"), agx.Vec3()))
# Set angular damping for segments
sim.add(dlo)
segment_iterator = dlo.begin()
n_segments = dlo.getNumSegments()
segments = []
for i in range(n_segments):
if not segment_iterator.isEnd():
seg = segment_iterator.getRigidBody()
segments.append(seg)
seg.setAngularVelocityDamping(2e4)
segment_iterator.inc()
mass_props = seg.getMassProperties()
mass_props.setMass(1.25*mass_props.getMass())
s0 = segments[0]
s1 = segments[-1]
h0 = agx.HingeFrame()
h0.setCenter(gripper_0.getRigidBody("gripper_0").getPosition())
h0.setAxis(agx.Vec3(0,0,1))
l0 = agx.Hinge(h0, s0, gripper_0.getRigidBody("gripper_0") )
sim.add(l0)
h1 = agx.HingeFrame()
h1.setCenter(gripper_1.getRigidBody("gripper_1").getPosition())
h1.setAxis(agx.Vec3(0,0,1))
l1 = agx.Hinge(h1, s1, gripper_1.getRigidBody("gripper_1") )
sim.add(l1)
# f0 = agx.Frame()
# f1 = agx.Frame()
# l0 = agx.LockJoint(s0, f0, gripper_0.getRigidBody("gripper_0"), f1)
# l1 = agx.LockJoint(s1, f0, gripper_1.getRigidBody("gripper_1"), f1)
# sim.add(l0)
# sim.add(l1)
# Try to initialize dlo
report = dlo.tryInitialize()
if report.successful():
print("Successful dlo initialization.")
else:
print(report.getActualError())
# Add rope to simulation
sim.add(dlo)
# Set rope material
material_rubber_band = dlo.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_0 = sim.getConstraint1DOF("gripper_0_joint_base_x").getMotor1D()
motor_y_0 = sim.getConstraint1DOF("gripper_0_joint_base_y").getMotor1D()
motor_x_1 = sim.getConstraint1DOF("gripper_1_joint_base_x").getMotor1D()
motor_y_1 = sim.getConstraint1DOF("gripper_1_joint_base_y").getMotor1D()
key_motor_map = {agxSDK.GuiEventListener.KEY_Up: (motor_y_0, 0.5),
agxSDK.GuiEventListener.KEY_Down: (motor_y_0, -0.5),
agxSDK.GuiEventListener.KEY_Right: (motor_x_0, 0.5),
agxSDK.GuiEventListener.KEY_Left: (motor_x_0, -0.5),
120: (motor_x_1, 0.5),
60: (motor_x_1, -0.5),
97: (motor_y_1, 0.5),
121: (motor_y_1, -0.5)}
sim.add(KeyboardMotorHandler(key_motor_map))
rbs = dlo.getRigidBodies()
for i in range(len(rbs)):
rbs[i].setName('dlo_' + str(i+1))
return sim
def build_simulation(goal=False, rope=True):
"""Builds simulations for both start and goal configurations
:param bool goal: toggles between simulation definition of start and goal configurations
:param bool rope: add rope to the scene or not
: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)
# Define materials
material_ground = agx.Material("Aluminum")
bulk_material = material_ground.getBulkMaterial()
bulk_material.setPoissonsRatio(ALUMINUM_POISSON_RATIO)
bulk_material.setYoungsModulus(ALUMINUM_YOUNG_MODULUS)
surface_material = material_ground.getSurfaceMaterial()
surface_material.setRoughness(GROUND_ROUGHNESS)
surface_material.setAdhesion(GROUND_ADHESION, GROUND_ADHESION_OVERLAP)
material_pusher = agx.Material("Aluminum")
bulk_material = material_pusher.getBulkMaterial()
bulk_material.setPoissonsRatio(ALUMINUM_POISSON_RATIO)
bulk_material.setYoungsModulus(ALUMINUM_YOUNG_MODULUS)
surface_material = material_pusher.getSurfaceMaterial()
surface_material.setRoughness(PUSHER_ROUGHNESS)
surface_material.setAdhesion(PUSHER_ADHESION, PUSHER_ADHESION_OVERLAP)
# Create a ground plane and bounding box to prevent falls
ground = create_body(name="ground" + goal_string, shape=agxCollide.Box(GROUND_LENGTH_X, GROUND_LENGTH_Y,
GROUND_WIDTH),
position=agx.Vec3(0, 0, -GROUND_WIDTH / 2),
motion_control=agx.RigidBody.STATIC,
material=material_ground)
scene.add(ground)
bounding_box = create_body(name="bounding_box_1" + goal_string, shape=agxCollide.Box(GROUND_LENGTH_X, GROUND_WIDTH,
RADIUS * 4),
position=agx.Vec3(0, GROUND_LENGTH_Y - GROUND_WIDTH, RADIUS * 4 - GROUND_WIDTH),
motion_control=agx.RigidBody.STATIC,
material=material_ground)
scene.add(bounding_box)
bounding_box = create_body(name="bounding_box_2" + goal_string, shape=agxCollide.Box(GROUND_LENGTH_X, GROUND_WIDTH,
RADIUS * 4),
position=agx.Vec3(0, - GROUND_LENGTH_Y + GROUND_WIDTH, RADIUS * 4 - GROUND_WIDTH),
motion_control=agx.RigidBody.STATIC,
material=material_ground)
scene.add(bounding_box)
bounding_box = create_body(name="bounding_box_3" + goal_string, shape=agxCollide.Box(GROUND_WIDTH, GROUND_LENGTH_Y,
RADIUS * 4),
position=agx.Vec3(GROUND_LENGTH_X - GROUND_WIDTH, 0, RADIUS * 4 - GROUND_WIDTH),
motion_control=agx.RigidBody.STATIC,
material=material_ground)
scene.add(bounding_box)
bounding_box = create_body(name="bounding_box_4" + goal_string, shape=agxCollide.Box(GROUND_WIDTH, GROUND_LENGTH_Y,
RADIUS * 4),
position=agx.Vec3(- GROUND_LENGTH_X + GROUND_WIDTH, 0, RADIUS * 4 - GROUND_WIDTH),
motion_control=agx.RigidBody.STATIC,
material=material_ground)
scene.add(bounding_box)
if rope:
# Create rope
rope = agxCable.Cable(RADIUS, RESOLUTION)
rope.add(agxCable.FreeNode(GROUND_LENGTH_X / 2 - RADIUS * 2, GROUND_LENGTH_Y / 2 - RADIUS * 2, RADIUS * 2))
rope.add(agxCable.FreeNode(GROUND_LENGTH_X / 2 - RADIUS * 2, GROUND_LENGTH_Y / 2 - LENGTH - RADIUS * 2,
RADIUS * 2))
# Set rope name and properties
rope.setName("DLO" + goal_string)
properties = rope.getCableProperties()
properties.setYoungsModulus(YOUNG_MODULUS_BEND, agxCable.BEND)
properties.setYoungsModulus(YOUNG_MODULUS_TWIST, agxCable.TWIST)
properties.setYoungsModulus(YOUNG_MODULUS_STRETCH, agxCable.STRETCH)
# Try to initialize rope
report = rope.tryInitialize()
if report.successful():
print("Successful rope initialization.")
else:
print(report.getActualError())
# Add rope to simulation
scene.add(rope)
rbs = rope.getRigidBodies()
for i in range(len(rbs)):
rbs[i].setName('dlo_' + str(i+1) + goal_string)
# Set rope material
material_rope = rope.getMaterial()
material_rope.setName("rope_material")
bulk_material = material_rope.getBulkMaterial()
bulk_material.setDensity(ROPE_DENSITY)
surface_material = material_rope.getSurfaceMaterial()
surface_material.setRoughness(ROPE_ROUGHNESS)
surface_material.setAdhesion(ROPE_ADHESION, 0)
# Check mass
rope_mass = rope.getMass()
print("Rope mass: {}".format(rope_mass))
# Create contact materials
contact_material_ground_rope = sim.getMaterialManager().getOrCreateContactMaterial(material_ground,
material_rope)
contact_material_pusher_rope = sim.getMaterialManager().getOrCreateContactMaterial(material_pusher,
material_rope)
contact_material_ground_rope.setUseContactAreaApproach(True)
sim.add(contact_material_ground_rope)
sim.add(contact_material_pusher_rope)
rotation_cylinder = agx.OrthoMatrix3x3()
rotation_cylinder.setRotate(agx.Vec3.Y_AXIS(), agx.Vec3.Z_AXIS())
pusher = create_body(name="pusher" + goal_string,
shape=agxCollide.Cylinder(PUSHER_RADIUS, PUSHER_HEIGHT),
position=agx.Vec3(0.0, 0.0, PUSHER_HEIGHT / 2),
rotation=rotation_cylinder,
motion_control=agx.RigidBody.DYNAMICS,
material=material_pusher)
scene.add(pusher)
# Create base for pusher motors
prismatic_base = create_locked_prismatic_base("pusher" + goal_string, pusher.getRigidBody("pusher" + goal_string),
position_ranges=[(-GROUND_LENGTH_X, GROUND_LENGTH_X),
(-GROUND_LENGTH_Y, GROUND_LENGTH_Y),
(0., 3 * RADIUS)],
motor_ranges=[(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE),
(-MAX_MOTOR_FORCE, MAX_MOTOR_FORCE)])
scene.add(prismatic_base)
return sim