def __init__(self, world, space, pos, texture):
self.fuellevel = 1
self.rgtexture = texture[0]
self.lgtexture = texture[1]
self.bodytexture = texture[2]
leftgear = [pos[0] - 33, pos[1] + 25, 0]
rightgear = [pos[0] + 33, pos[1] + 25, 0]
self.mainbody = BodyBox(world, space, pos, (50, 50, 50), 15)
self.leftgearbody = BodyBox(world, space, leftgear, (12, 25, 25), 1)
self.rightgearbody = BodyBox(world, space, rightgear, (12, 25, 25), 1)
self.leftgearbody.assignTexture(self.lgtexture)
self.rightgearbody.assignTexture(self.rgtexture)
self.mainbody.assignTexture(self.bodytexture)
# atach gear to main body
self.leftjoint = ode.SliderJoint(world)
self.leftjoint.attach(self.mainbody.body, self.leftgearbody.body)
self.leftjoint.setAxis((0, 1, 0))
self.leftjoint.setParam(ode.paramLoStop, 0)
self.leftjoint.setParam(ode.paramHiStop, 50)
self.rightjoint = ode.SliderJoint(world)
self.rightjoint.attach(self.mainbody.body, self.rightgearbody.body)
self.rightjoint.setAxis((0, 1, 0))
self.rightjoint.setParam(ode.paramLoStop, 0)
self.rightjoint.setParam(ode.paramHiStop, 30)
self.carryingitem = 0
self.pickupdelay = pygame.time.get_ticks()
def __init__(self, gravity = 9.8, mass = 1.0, tau = 0.02, size_box = (0.5, 0.3), size_pole = (1.0, .1)): self.gravity = gravity self.mass = mass self.dt = tau self.viewer = None self.viewerSize = 500 self.spaceSize = 7. self.size_box = size_box self.size_pole = size_pole self.max_force = 10. self.theta_threshold = 3.2 self.x_threshold = 3.0 self.action_space = spaces.Box(low = -self.max_force, high = self.max_force, shape=(3,)) high = np.array([self.x_threshold * 2, np.finfo(np.float32).max, self.theta_threshold *2, np.finfo(np.float32).max, \ self.theta_threshold *2, np.finfo(np.float32).max, self.theta_threshold *2, np.finfo(np.float32).max]) self.observation_space = spaces.Box(-high, high) self.world = ode.World() self.world.setGravity((0, -9.81, 0)) self.body1 = ode.Body(self.world) self.body2 = ode.Body(self.world) self.body3 = ode.Body(self.world) self.body4 = ode.Body(self.world) self.create_basebox(self.body1, (0.,0.,0.), self.mass, size_box) self.create_link(self.body2, (0.,size_pole[0]/2,0.), self.mass, size_pole) self.create_link(self.body3, (0.,size_pole[0]+size_pole[0]/2,0.), self.mass, size_pole) self.create_link(self.body4, (0.,size_pole[0]*2+size_pole[0]/2,0.), self.mass, size_pole) self.space = ode.Space() self.j1 = ode.SliderJoint(self.world) self.j1.attach(self.body1, ode.environment) self.j1.setAxis( (1, 0, 0) ) self.j1.setFeedback(1) self.j2 = ode.HingeJoint(self.world) self.j2.attach(self.body1, self.body2) self.j2.setAnchor( (0., 0. ,0.) ) self.j2.setAxis ( (0, 0, -1) ) self.j2.setFeedback(1) self.j3 = ode.HingeJoint(self.world) self.j3.attach(self.body2, self.body3) self.j3.setAnchor( (0., size_pole[0] ,0.) ) self.j3.setAxis ( (0, 0, -1) ) self.j3.setFeedback(1) self.j4 = ode.HingeJoint(self.world) self.j4.attach(self.body3, self.body4) self.j4.setAnchor( (0., size_pole[0]*2 ,0.) ) self.j4.setAxis ( (0, 0, -1) ) self.j4.setFeedback(1)
def end(name):
if (name == 'slider'):
joint = ode.SliderJoint(world, self._jg)
joint.attach(link1, link2)
if (len(axes) != 1):
raise errors.InvalidError('Wrong number of axes for slider'
' joint.')
joint.setAxis(self._parser.parseVector(axes[0]))
self._applyAxisParams(joint, 0, axes[0])
self.setODEObject(joint)
self._parser.pop()
def build_world():
# Create the world through ode
density = 1
start_up = True
world = ode.World()
world.setGravity((0, -gravity, 0))
# Make the cart
cart = ode.Body(world)
M = ode.Mass()
M.setBox(density, cart_width, cart_height, cart_height)
M.mass = cart_mass
cart.setMass(M)
cart.setPosition((0, 0, 0))
print 'cart mass = ', cart.getMass().mass
# Make a heavy ball
ball = ode.Body(world)
M = ode.Mass()
M.setSphere(density, 0.5)
M.mass = ball_mass
ball.setMass(M)
ball.setPosition((0, 1, 0))
# And a rod with a negligible weight
rod = ode.Body(world)
M = ode.Mass()
M.setCylinder(0.01, 2, 0.01, rod_length)
M.mass = 1e-2
rod.setMass(M)
rod.setPosition((0, 0.5, 0))
## Connect the cart to the world through a slider joint
cart_joint = ode.SliderJoint(world)
cart_joint.setAxis((1, 0, 0))
cart_joint.attach(cart, ode.environment)
# Connect the rod with the cart through a Hinge joint.
pendulum_joint = ode.HingeJoint(world)
pendulum_joint.attach(rod, cart)
pendulum_joint.setAnchor((0, 0, 0))
pendulum_joint.setAxis((0, 0, 1))
# Connect rod with ball with a fixed joint
rod_ball_joint = ode.FixedJoint(world)
rod_ball_joint.attach(rod, ball)
rod_ball_joint.setFixed()
return world, cart, ball, rod, pendulum_joint, cart_joint, rod_ball_joint
def __init__(self, world: ode.World = None, visualized=False):
super(SlidingPendulum, self).__init__(world)
# Bodies and joints
# Create wagon
wagon = ode.Body(self.world)
M = ode.Mass()
M.setSphere(2500, 0.05)
wagon.setMass(M)
wagon.setPosition((0, 1, 0))
# Create pendulum
pendulum = ode.Body(self.world)
M = ode.Mass()
M.setSphere(2500, 0.05)
pendulum.setMass(M)
pendulum.setPosition((0, 2, 0))
# Connect wagon with the static environment using a slider joint
slider = ode.SliderJoint(self.world)
slider.attach(ode.environment, wagon)
slider.setAxis((1, 0, 0))
# Connect pendulum with wagon
arm = ode.HingeJoint(self.world)
arm.attach(wagon, pendulum)
arm.setAnchor(wagon.getPosition())
arm.setAxis((0, 0, 1))
self._wagon = wagon
self._pendulum = pendulum
self._slider = slider
self._arm = arm
slider.setParam(ode.ParamVel, 0.1)
slider.setParam(ode.ParamFMax, 22) # used to be 22
# visualization
self._visualized = visualized
if visualized:
surface = pygame.display.set_mode((640, 480))
SimMan.process(self._screenUpdater(surface))
def __init__(self, world, body1, body2, p1, p2, hinge=None):
# Cap on one side
cap1 = ode.Body(world)
cap1.color = (0, 128, 0, 255)
m = ode.Mass()
m.setCappedCylinderTotal(1.0, 3, 0.01, 0.01)
cap1.setMass(m)
# set parameters for drawing the body
cap1.shape = "capsule"
cap1.length = .05
cap1.radius = .05
cap1.setPosition(p1)
# Attach the cap to the body
u1 = ode.BallJoint(world)
u1.attach(body1, cap1)
u1.setAnchor(p1)
u1.style = "ball"
# Cap on other side
cap2 = ode.Body(world)
cap2.color = (0, 128, 0, 255)
m = ode.Mass()
m.setCappedCylinderTotal(1.0, 3, 0.01, 0.01)
cap2.setMass(m)
# set parameters for drawing the body
cap2.shape = "capsule"
cap2.length = .05
cap2.radius = .05
cap2.setPosition(p2)
# Attach the cap to the body
u2 = ode.BallJoint(world)
u2.attach(body2, cap2)
u2.setAnchor(p2)
u2.style = "ball"
# The all-important slider joint
s = ode.SliderJoint(world)
s.attach(cap1, cap2)
s.setAxis(sub3(p1, p2))
s.setFeedback(True)
self.body1 = body1
self.body2 = body2
self.cap1 = cap1
self.cap2 = cap2
self.u1 = u1
self.u2 = u2
self.slider = s
self.gain = 1.0
self.neutral_length = dist3(p1, p2)
self.length_target = dist3(p1, p2)
if hinge is not None:
# Hinge is the joint this linear actuator controls
# This allows angular control
self.hinge = hinge
# Store these for later to save on math.
# TODO: I am being lazy and assuming u1->u2
# is orthogonal to the hinge axis
self.h_to_u1 = dist3(self.hinge.getAnchor(), self.u1.getAnchor())
self.h_to_u2 = dist3(self.hinge.getAnchor(), self.u2.getAnchor())
self.neutral_angle = thetaFromABC(self.h_to_u1, self.h_to_u2,
self.neutral_length)
body1 = ode.Body(world)
M = ode.Mass()
M.setBox(250, 1, 0.5, 0.1)
M.mass = 1.0
body1.setMass(M)
body1.setPosition((0, 0, 0))
body1.setForce((1, 0, 0))
body2 = ode.Body(world)
M = ode.Mass()
M.setBox(2.5, 0.2, 2, 0.2)
M.mass = 0.1
body2.setMass(M)
body2.setPosition((0, 0.5, 0))
j1 = ode.SliderJoint(world)
j1.attach(body1, ode.environment)
j1.setAxis((1, 0, 0))
j2 = ode.HingeJoint(world)
j2.attach(body1, body2)
j2.setAnchor((0, 0, 0))
j2.setAxis((0, 0, 1))
class CartPoleODEEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second': 50
}
def __init__(self, max_simsecs=30, net=None, noise_sd=0.01, quick=0):
"""Creates the ODE and Geom bodies for this simulation"""
Sim.__init__(self, max_simsecs, noise_sd, quick)
log.debug('init PoleBalance sim')
self.network = net
CART_POSITION = (0, 0, 2)
POLE_POSITION = (0, 0, 3 + (5.0 / 2))
CART_SIZE = (8, 8, 2)
POLE_SIZE = (1, 1, 5)
HINGE_POSITION = (0, 0, 3)
CART_MASS = 10
POLE_MASS = 0.5
self.MAXF = 1000
self.INIT_U = [] # initial force, eg [10000]
self.cart_geom = ode.GeomBox(self.space, CART_SIZE)
self.geoms.append(self.cart_geom)
self.cart_body = ode.Body(self.world)
self.cart_body.setPosition(CART_POSITION)
cart_mass = ode.Mass()
cart_mass.setBoxTotal(CART_MASS, CART_SIZE[0], CART_SIZE[1],
CART_SIZE[2])
self.cart_body.setMass(cart_mass)
self.cart_geom.setBody(self.cart_body)
self.pole_geom = ode.GeomBox(self.space, POLE_SIZE)
self.geoms.append(self.pole_geom)
self.pole_body = ode.Body(self.world)
self.pole_body.setPosition(POLE_POSITION)
pole_mass = ode.Mass()
pole_mass.setBoxTotal(POLE_MASS, POLE_SIZE[0], POLE_SIZE[1],
POLE_SIZE[2])
self.pole_body.setMass(pole_mass)
self.pole_geom.setBody(self.pole_body)
self.cart_geom.setCategoryBits(long(0))
self.pole_geom.setCategoryBits(long(0))
# joint 0 - slide along 1D
self.slider_joint = ode.SliderJoint(self.world)
self.joints.append(self.slider_joint)
self.slider_joint.attach(self.cart_body, ode.environment)
self.slider_joint.setAxis((1, 0, 0))
self.slider_joint.setParam(ode.ParamLoStop, -5)
self.slider_joint.setParam(ode.ParamHiStop, 5)
# joint 1 - hinge between the two boxes
self.hinge_joint = ode.HingeJoint(self.world)
self.joints.append(self.hinge_joint)
self.hinge_joint.attach(self.cart_body, self.pole_body)
self.hinge_joint.setAnchor(HINGE_POSITION)
self.hinge_joint.setAxis((0, 1, 0))
self.last_hit = 0.0
self.init_u_count = 0
self.regular_random_force = 1
self.lqr = None
self.controlForce = 0
self.randomForce = 0
self.force_urge = 0
def _createODEjoint(self):
# Create the ODE joint
self.odejoint = ode.SliderJoint(self.odedynamics.world)
