def seesaw(sim, root, pos, angle, h=0.08):
d = 0.8
# Sides
dims = [0.6*(width/14), 0.15*(width/14), h*3/2]
pos_s = [pos[0]+(d/2+0.3)*np.cos(angle)*(width/14), pos[1]+(d/2+0.3)*np.sin(angle)*(width/14), pos[2]+h/2]
sideP = addboxx(sim, root, dims, pos_s)
dims = [0.6*(width/14), 0.15*(width/14), h*3/2]
pos_s = [pos[0]-(d/2+0.3)*np.cos(angle)*(width/14), pos[1]-(d/2+0.3)*np.sin(angle)*(width/14), pos[2]+h/2]
sideN = addboxx(sim, root, dims, pos_s)
# Main board
dims = [d*(width/14), 0.9*(width/14), 0.004]
pos_s = [pos[0]+0.06*np.sin(angle)*(width/14), pos[1]-0.06*np.cos(angle)*(width/14), pos[2]+h]
board = addboxx(sim, root, dims, pos_s, Fixed=False)
sideP.setRotation(agx.Quat(angle, agx.Vec3(0,0,1)))
sideN.setRotation(agx.Quat(angle, agx.Vec3(0,0,1)))
board.setRotation(agx.Quat(angle, agx.Vec3(0,0,1)))
#Some stops under
bot_tilt = 0.17
dims = [d, 0.43, 0.004]
dif = 0.215*np.cos(bot_tilt)*(width/14)-0.002*np.sin(bot_tilt)*(width/14)
pos_s = [pos[0]+(0.06+dif)*np.sin(angle)*(width/14), pos[1]-(0.06+dif)*np.cos(angle)*(width/14), pos[2]+np.sin(bot_tilt)*dif]
bottom1 = addboxx(sim, root, dims, pos_s, color=agxRender.Color.DarkGray())
pos_s = [pos[0]+(0.06-dif)*np.sin(angle)*(width/14), pos[1]-(0.06-dif)*np.cos(angle)*(width/14), pos[2]+np.sin(bot_tilt)*dif]
bottom2 = addboxx(sim, root, dims, pos_s, color=agxRender.Color.DarkGray())
bottom1.setRotation(agx.Quat( bot_tilt, agx.Vec3(1,0,0)))
bottom1.setRotation(bottom1.getRotation()*agx.Quat(angle, agx.Vec3(0,0,1)))
bottom2.setRotation(agx.Quat(-bot_tilt, agx.Vec3(1,0,0)))
bottom2.setRotation(bottom2.getRotation()*agx.Quat(angle, agx.Vec3(0,0,1)))
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3( np.cos(angle),np.sin(angle),0))
hf.setCenter(agx.Vec3(pos[0]+(d/2)*np.cos(angle)*(width/14), pos[1]+(d/2)*np.sin(angle)*(width/14), pos[2]+h))
axleP = agx.Hinge(hf, board, sideP)
sim.add(axleP)
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3( np.cos(angle),np.sin(angle),0))
hf.setCenter(agx.Vec3(pos[0]*(width/14)-(d/2)*np.cos(angle)*(width/14), pos[1]*(width/14)-(d/2)*np.sin(angle)*(width/14), pos[2]+h))
axleN = agx.Hinge(hf, board, sideN)
sim.add(axleN)
def LinkBodies_Hinge(body1, body2, link_position, link_direction, MiroSystem=False):
hf = agx.HingeFrame()
hf.setAxis(agxVecify(link_direction))
hf.setCenter(agxVecify(link_position))
link = agx.Hinge(hf, body1, body2)
# The line below is disabled because it messes up the steering of the example bot.
# link.setSolveType(agx.Constraint.DIRECT_AND_ITERATIVE)
if MiroSystem:
MiroSystem.Add(link)
return link
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 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 obstacles(sim, root, h):
#start plattform
dims = [1.5, 1.5, 0.06]
pos = [-6, 0, h + dims[2] / 2]
startbox = addboxx(sim, root, dims, pos)
#timer
timer = Timer(startbox)
sim.add(timer)
dims = [0.1, 1.5, 0.3]
pos = [-6.75, 0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.1, 1.5, 0.3]
pos = [-5.25, 0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [1.6, 0.1, 0.3]
pos = [-6, -0.75, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
# wall
dims = [4.0, 0.1, 0.22]
pos = [-3.3, -0.6, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
# Random stuff in the first quarter
dims = [0.2, 0.2, 0.8]
pos = [-4, 0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.4, 0.25, 0.2]
pos = [-3, 1.5, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
for i in range(30):
x = -0.5 - random.random() * 4.75
y = -0.5 + random.random() * 6
dims = [
random.random() * 0.6,
random.random() * 0.6,
random.random() * 0.6
]
pos = agx.Vec3(x, y, 0)
if pos.length() < 1.5:
pos.setLength(1.5 + random.random() * 3.75)
if pos.length() > 7.0:
pos.setLength(1.5 + random.random() * 5.5)
pos.set(h + dims[2] / 2, 2)
addboxx(sim, root, dims, pos)
# Pole in the middle with walls around
dims = [0.28, 1.4]
pos = [0, 0, h + dims[1] / 2]
can = addcylinder(sim, root, dims, pos, texture='schrodbull.png')
can.setRotation(agx.Quat(np.pi / 2, agx.Vec3(1, 0, 0)))
pos = [0, 0, h + dims[1]]
dims = [0.28, 0.025]
pos[2] = pos[2] + dims[1] / 2
lid = addcylinder(sim, root, dims, pos, texture='sodacan_lid.png')
lid.setRotation(agx.Quat(np.pi / 2, agx.Vec3(1, 0, 0)))
dims = [0.3, 2.0, 0.4]
pos = [-1.15, 0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [2.6, 0.3, 0.4]
pos = [0, -1.15, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.3, 2.0, 0.4]
pos = [1.15, 0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.3, 7.0, 0.4]
pos = [0, 3.5, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
# Seesaw board
dims = [2.1, 0.25, 0.3]
pos = [2.1, 0.4, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
seesaw(sim, root, [3.75, 0.9, h], -0.85 * np.pi, h=0.1)
dims = [0.5, 3.8, 0.18]
pos = [4.8, 3.15, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
# Ballroom walls
dims = [0.25, 4.3, 0.3]
pos = [6.0, 0.0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.25, 1.0, 0.3]
pos = [2.1, -1.0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.25, 1.4, 0.3]
pos = [3.0, -0.4, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.25, 2.4, 0.3]
pos = [2.5, -3.0, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [1.1, 0.25, 0.3]
pos = [3.1, -3.1, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [0.66, 0.25, 0.3]
pos = [2.3, -1.65, h + dims[2] / 2]
wall_1 = addboxx(sim, root, dims, pos)
dims = [2.8, 0.25, 0.3]
pos = [3.95, -2.0, h + dims[2] / 2]
wall_2 = addboxx(sim, root, dims, pos)
dims = [3.91, 0.25, 0.3]
pos = [4.65, -3.45, h + dims[2] / 2]
wall_3 = addboxx(sim, root, dims, pos)
wall_1.setRotation(agx.Quat(-np.pi / 4, agx.Vec3(0, 0, 1)))
wall_2.setRotation(agx.Quat(-np.pi / 4, agx.Vec3(0, 0, 1)))
wall_3.setRotation(agx.Quat(np.pi / 4, agx.Vec3(0, 0, 1)))
# Ballroom balls
for i in range(200):
x = 4.0 + random.random() * 2.8
y = 0.75 - random.random() * 2.5
rad = 0.025 + random.random() * 0.075
pos = agx.Vec3(x, y, h + rad + 3 * random.random() * rad)
addball(sim, root, rad, pos, Fixed=False)
# Climbing ramp
dx = 0.8
bot_tilt = 0.0445
dims = [dx, 2.5, 0.6]
bot_tilt = np.arcsin(0.45 / (2.5 * 4))
dif = dims[1] / 2 * np.cos(bot_tilt) - 0.002 * np.sin(bot_tilt)
dh = 2 * np.sin(bot_tilt) * dif
for i in range(4):
angle = (i + 1) * np.pi
pos = [2 - dx * i, -4.7 - 0.015 * (-1)**i, -0.3 + h + (i + 1 / 2) * dh]
hip = addboxx(sim, root, dims, pos)
hip.setRotation(agx.Quat(bot_tilt, agx.Vec3(1, 0, 0)))
hip.setRotation(hip.getRotation() * agx.Quat(angle, agx.Vec3(0, 0, 1)))
addboxx(sim, root, [2 * dx, 1.1, dims[2]], [
2 - dx * (i + 1 / 2), -4.7 - 1.8 * ((-1)**i), -0.3 + h +
(i + 1) * dh
])
# Bridge boxes
dims = [1.5, 1.8, 0.45]
pos = [-1.5, -3.25, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
dims = [1.5, 1.8, 0.45]
pos = [-4.5, -3.25, h + dims[2] / 2]
addboxx(sim, root, dims, pos)
# Bridge part
bridge = addboxx(sim, root, [1.5, 0.5, 0.08],
[-3.0, -3.25, h + 0.45 - 0.04])
timer.addCheckpoint(bridge)
# Swinging ball over bridge
rad = 0.4
pendulum = addball(sim,
root,
rad, [-3.0, -3.25, h + 0.45 + rad + 0.01],
Fixed=False)
pendulum.setVelocity(agx.Vec3(0, 5, 0))
hf = agx.HingeFrame()
hf.setAxis(agx.Vec3(1, 0, 0))
hf.setCenter(agx.Vec3(-3.0, -3.25, 3.0 + h + 0.45 + rad + 0.01))
axleP = agx.Hinge(hf, pendulum)
sim.add(axleP)
# addboxx(sim, root, [1.5, 0.5, 0.08], )
# final ramp
addboxx(sim, root, [0.1, 2.5, 0.5], [-6.5, -2.05, h + 0.5 / 2])
addboxx(sim, root, [0.1, 2.5, 0.5], [-5.5, -2.05, h + 0.5 / 2])
hip = addboxx(sim, root, [0.9, 1.5, 0.1],
[-6.0, -1.53, h + 0.026 + np.sin(0.15) * 1.5 / 2])
hip.setRotation(agx.Quat(0.15, agx.Vec3(1, 0, 0)))
addboxx(sim, root, [0.9, 2.0, 0.2], [-6.0, -2.25, h + 0.075])
addboxx(sim, root, [1.4, 0.8, 0.2], [-5.8, -3.7, h + 0.45 - 0.1])
hip = addboxx(sim, root, [0.9, 1.5, 0.1],
[-6.0, -3.0, h + 0.026 + np.sin(0.15) * 1.5 / 2])
hip.setRotation(agx.Quat(-0.15, agx.Vec3(1, 0, 0)))
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