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)