def __init__(self):
#CACHES
self.physicals = [] #static and moving - for collision tests
self.movings = [] #moving only
self.robots = [] #things with controllers
self.rawGLObjects = [] #things that need to call special graphics
#ODE
self.world = ode.World()
self.world.setGravity((0, 0, 0)) #(0,-9.81,0)
self.space = ode.Space() #for environment object geoms
self.segSpace = ode.Space(
) #keep segs in sep space to reduce number of comparisons
self.robotSpace = ode.Space() #for the robot body parts
self.contactgroup = ode.JointGroup()
#INVENTOR
root = SoSeparator(
) #collide((u.world, u.contactgroup), contact_callback) #TODO replacewith custom collisions, segs vs whole world and robot vs whole world
root.addChild(SoDirectionalLight())
self.myCamera = SoPerspectiveCamera()
root.addChild(self.myCamera)
myMaterial = SoMaterial()
myMaterial.diffuseColor = (1.0, 0.0, 0.0) # Red
root.addChild(myMaterial)
env = SoSeparator() #the environent (ie not robots)
root.addChild(env)
self.root = root
#CONTROLLER TIMER
self.controllerTimer = 0
def __init__(self, gravity=9.8, mass=1.0, tau=0.02, 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_pole = size_pole
self.max_force = 3.
self.theta_threshold = 3.2
self.action_space = spaces.Box(low=-self.max_force,
high=self.max_force,
shape=(1, ))
high = np.array([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.create_basebox(self.body1, (0., 0., 0.))
self.create_link(self.body2, (0., 0.5, 0.), self.mass, size_pole)
self.space = ode.Space()
self.j1 = ode.FixedJoint(self.world)
self.j1.attach(self.body1, ode.environment)
self.j1.setFixed()
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)
def setupSimulation(N):
global world, walls, space, geoms, objs
world = ode.World()
space = ode.Space()
world.setERP(0.8)
world.setCFM(1E-5)
objs = []
planeConstraint = ode.Plane2DJoint(world)
for i in range(N):
ball = ode.Body(world)
M = ode.Mass()
M.setSphere(2500.0, 0.05)
M.mass = 2
ball.setMass(M)
bx = (np.random.rand() * 2 - 1) * 0.7
by = (np.random.rand() * 2 - 1) * 0.7
ball.setPosition((bx, by, 0))
geom = ode.GeomSphere(space, radius=0.2)
geom.setBody(ball)
geoms.append(geom)
planeConstraint.attach(ball, ode.environment)
objs.append(ball)
walls = []
walls.append(ode.GeomPlane(space, (0, 1, 0), -1))
walls.append(ode.GeomPlane(space, (1, 0, 0), -1))
walls.append(ode.GeomPlane(space, (0, -1, 0), -1))
walls.append(ode.GeomPlane(space, (-1, 0, 0), -1))
def __init__(self, robotfile, standalone=0,obstaclefile=None,regionfile=None,region_calib=None,startingpose=None,heightmap=None): """ Initialize the simulator. """ # Simulation world parameters. self._initOpenGL() self.world = ode.World() self.world.setGravity((0, -9.81, 0)) self.world.setERP(0.1) self.space = ode.Space() self.ground = ode.GeomPlane(space=self.space, normal=(0,1,0), dist=0) # CKBot module parameters. self.cubesize = 6.0 self.cubemass = 10.0 self.hingemaxangle = 1.6 self.hingeminangle = -1.6 self.hingemaxforce = 1000000 # Control parameters. self.gait = 0 self.gain = 1.5 self.counter = 0 # Setting standalone to 1 allows for manual key input. # Setting standalone to 0 (LTLMoP mode) causes the camera to automatically follow the spawned robot) if standalone==1: self.standalone = 1 else: self.standalone = 0 self.clock = pygame.time.Clock()
def __init__(self,
dt=argv.get("dt", 0.02),
mu=5000,
floor_mu=None,
bounce=1.2,
has_floor=True,
has_gravity=True,
threshold=0.001,
ERP=0.8,
CFM=1e-5):
world = ode.World()
if has_gravity:
world.setGravity((0, -9.81, 0))
world.setERP(ERP)
world.setCFM(CFM)
#world.setCFM(1e-4) # more spongey
space = ode.Space()
if has_floor:
self.floor = ode.GeomPlane(space, (0, 1, 0), 0)
else:
self.floor = None
self.world = world
self.space = space
self.items = []
self.transient = []
self.dt = dt
self.mu = mu
self.threshold = threshold
if floor_mu is None:
floor_mu = mu
self.floor_mu = floor_mu
self.bounce = bounce
def __init__(self, dt=(1 / 45.)):
'''This section incorporates PyODE to define methods to create and draw three objects: a sphere, box, or cylinder.
Some code borrowed from http://sourceforge.net/p/pyode/mailman/message/19674697/.
'''
self.dt = dt
self.entities = []
# Create a world object
self.ode_world = ode.World()
self.ode_world.setGravity(np.array([0, 0, Constants.g]))
try:
self.ode_world.setAngularDamping(0.2)
except AttributeError:
rospy.logerr(
"SIM ERROR: You need to re-run the install script, or manually fix pyode"
)
rospy.logerr("SIM ERROR: Killing simulation")
exit(0)
# self.ode_world.setLinearDamping(0.2)
self.ode_world.setERP(0.8)
self.ode_world.setCFM(1E-5)
# Create a space object
self.space = ode.Space()
self.contact_group = ode.JointGroup()
self.ode_world.step(self.dt)
def start(cls, gravity=(0, -9.8, 0), erp=.8, cfm=1e-5):
cls.world = ode.World()
cls.world.setGravity(gravity)
cls.world.setERP(erp)
cls.world.setCFM(cfm)
cls.space = ode.Space()
cls.contactgroup = ode.JointGroup()
def __init__(self, logfile, stepsize=0.005):
self.logfile = logfile
# Create a world object
self.world = ode.World()
self.world.setGravity((0, 0, 0))
self.world.setERP(0.5)
self.world.setCFM(1E-4)
# Create a space object
self.space = ode.Space()
# Create a plane geom which prevent the objects from falling forever
self.floor = ode.GeomPlane(self.space, (0, 1, 0), 0)
# A list of ODE Bodies
self.bodies = {}
# A list of geoms for the bodies
self.geoms = {}
# Set the stepsize for the instance.
self.stepsize = stepsize
self.positions = []
self.quaternions = []
self.curstep = 0
self.read_file()
def initialize(self):
if not SubSystem.initialize(self):
return False
# Crating ode world
self._world = ode.World()
self._world.setGravity((0, -9.81, 0))
self._space = ode.Space()
self._contactGroup = ode.JointGroup()
def __init__(self, gravity=-9.81):
# Create a world object
self.world = ode.World()
self.space = ode.Space()
self.contactgroup = ode.JointGroup()
self.world.setGravity((0, gravity, 0))
#self.world.setCFM(0.000001)
self.world.setERP(.99)
def reset(self):
self._world = ode.World()
self._space = ode.Space()
self._obstacles = []
self._contacts = ode.JointGroup()
self._drill = None
self._drill_offset = None
self._collision_detected = False
self._dirty = True
def __init__(self, col_callback, stepsize=0.005,log_data=False,output_path="",gravity=-9.81,fluid_dynamics=0,run_num=0,erp=0.5,cfm=1E-4):
# Create a world object
self.world = ode.World()
self.world.setGravity((0,gravity,0) )
self.world.setERP(erp)
self.world.setCFM(cfm)
self.world.setContactMaxCorrectingVel(5.)
# Run Number
self.run_num = run_num
# Create a space object
self.space = ode.Space()
# Create a plane geom which prevent the objects from falling forever
self.floor = ode.GeomPlane(self.space, (0,1,0), 0)
# A joint group for the contact joints that are generated whenever
# two bodies collide
self.contactgroup = ode.JointGroup()
# A list of ODE Bodies
self.bodies = {}
# Maintain a data structure of the surfaces of each body if fluid dynamics are required.
self.fluid_dynamics=fluid_dynamics
if(fluid_dynamics):
self.surfaces = {}
# A list of geoms for the bodies
self.geoms = {}
# A dict of terrain geoms for the world.
self.terrain_geoms = {}
# A list of joints in the world.
self.joints = {}
# Register the collision callback
self.col_callback = col_callback
# Set the stepsize for the instance.
self.stepsize = stepsize
# Create a cushion for joint limits.
self.joint_cushion = 0.#5.
# Create a maximum velocity that a joint is able to move in one step.
self.max_joint_vel = 36000. * self.stepsize*ANG_TO_RAD
# Logging functionality
self.log_data = log_data
self.output_path = output_path
if self.log_data:
self.positions = []
self.quaternions = []
self.joint_angles = []
def initPhysics(self):
import ode
# Create a default world and a space
self.world = ode.World()
self.world.setGravity((0, -9.81, 0))
self.world.setERP(0.8)
self.world.setCFM(1e-5)
self.space = ode.Space()
self.contactGroup = ode.JointGroup()
def createWorld(self):
#create world
self.world = ode.World()
self.world.setGravity(GRAVITY)
self.world.setERP(0.8)
self.world.setCFM(1E-5)
#create Space
self.space = ode.Space()
# Create a plane geom which prevent the objects from falling forever
self.floor = ode.GeomPlane(self.space, (0, 1, 0), 0)
def make_world(grav=(0, -9.81, 0)):
world = ode.World()
world.setGravity(grav)
world.setERP(0.8)
world.setCFM(1E-5)
space = ode.Space()
bodies = []
geoms = []
contactgroup = ode.JointGroup()
return world, space, bodies, geoms, contactgroup
def __init__(self):
viz.ODE_Visualization.__init__(self, ode.World(), [ode.Space()])
self.GetActiveCamera().SetPosition(534.532069842, -2309.40051692,
724.034189778)
self.GetActiveCamera().SetFocalPoint(0, 0, 0)
self.GetActiveCamera().SetViewUp(-0.00906125226669, 0.29723565994900,
0.95476115136800)
self.MainTitle = "GpsSatFi"
self.SetWindowName(self.MainTitle)
def reset(self):
# create an ODE world object
global world
world = ode.World()
world.setGravity((0.0, -9.81, 0.0))
world.setERP(0.1)
world.setCFM(1E-4)
# create an ODE space object
global space
space = ode.Space()
# create a plane geom to simulate a floor
floor = ode.GeomPlane(space, (0, 1, 0), 0)
# create a list to store any ODE bodies which are not part of the ragdoll (this
# is needed to avoid Python garbage collecting these bodies)
global bodies
bodies = []
# create a joint group for the contact joints generated during collisions
# between two bodies collide
global contactgroup
contactgroup = ode.JointGroup()
# set the initial simulation loop parameters
global Paused, lasttime, numiter, fps, dt, SloMo, stepsPerFrame
fps = 60
dt = 1.0 / fps
stepsPerFrame = 1
SloMo = 1
Paused = False
lasttime = time.time()
numiter = 0
# reward.
global reward
reward = 0
# create the pendulum
global pendulum
pendulum = pd.Pendulum(world, space, 500, (0.5, 0.0, 0.3))
print "total mass is %.1f kg (%.1f lbs)" % (pendulum.totalMass,
pendulum.totalMass * 2.2)
global TARGET_ANGLE
TARGET_ANGLE = pendulum.hHinge.getAngle()
# set GLUT callbacks
#glutKeyboardFunc(self.onKey)
#glutDisplayFunc(onDraw)
#glutIdleFunc(onIdle)
# enter the GLUT event loop
self.SpinOnce()
def __init__(self,density=1.0,bar_center=(0.0,0.0,0.0),bar_dim=(1.0,1.0,1.0),barycenters=None,he=1.0,cfl_target=0.9,dt_init=0.001):
self.dt_init = dt_init
self.he=he
self.cfl_target=cfl_target
self.world = ode.World()
#self.world.setERP(0.8)
#self.world.setCFM(1E-5)
self.world.setGravity(g)
self.g = np.array([g[0],g[1],g[2]])
self.space = ode.Space()
eps_x = L[0]- 0.75*L[0]
eps_y = L[1]- 0.75*L[1]
#tank geometry
#self.tankWalls = [ode.GeomPlane(self.space, (1,0,0) ,x_ll[0]+eps_x),
# ode.GeomPlane(self.space, (-1,0,0),-(x_ll[0]+L[0]-eps_x)),
#ode.GeomPlane(self.space, (0,1,0) ,x_ll[1]+eps_y),
# ode.GeomPlane(self.space, (0,-1,0) ,-(x_ll[1]+L[1]-eps_y))]
#mass/intertial tensor of rigid bar
#eps_x = L[0]- 0.45*L[0]
#eps_y = L[1]- 0.45*L[1]
#self.tankWalls = [ode.GeomPlane(space, (1,0,0) ,x_ll[0]+eps_x),
# ode.GeomPlane(space, (-1,0,0),-(x_ll[0]+L[0]-eps_x)),
# ode.GeomPlane(space, (0,1,0) ,x_ll[1]+eps_y),
# ode.GeomPlane(space, (0,-1,0) ,-(x_ll[1]+L[1]-eps_y))]
#self.tank = ode.GeomBox(self.space,(0.45,0.45,0.3))
#self.tank.setPosition((0.5,0.5,0.6))
#self.contactgroup = ode.JointGroup()
self.M = ode.Mass()
self.totalMass = density*bar_dim[0]*bar_dim[1]*bar_dim[2]
self.M.setBox(density,bar_dim[0],bar_dim[1],bar_dim[2])
#bar body
self.body = ode.Body(self.world)
self.body.setMass(self.M)
self.body.setFiniteRotationMode(1)
#bar geometry
self.bar = ode.GeomBox(self.space,bar_dim)
self.bar.setBody(self.body)
self.bar.setPosition(bar_center)
self.boxsize = (bar_dim[0],bar_dim[1],bar_dim[2])
#contact joints
self.contactgroup = ode.JointGroup()
self.last_position=bar_center
self.position=bar_center
self.last_velocity=(0.0,0.0,0.0)
self.velocity=(0.0,0.0,0.0)
self.h=(0.0,0.0,0.0)
self.rotation = np.eye(3)
self.last_rotation = np.eye(3)
self.last_rotation_inv = np.eye(3)
self.barycenters=barycenters
self.init=True
self.bar_dim = bar_dim
self.last_F = np.zeros(3,'d')
self.last_M = np.zeros(3,'d')
def __init__(self,
gravity=9.8,
mass_cart=1.0,
mass_pole=1.0,
tau=0.02,
size_box=(0.5, 0.3),
size_pole=(1.0, .1)):
self.gravity = gravity
self.mass_pole = mass_pole
self.mass_cart = mass_cart
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=(1, ))
high = np.array([
self.x_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, -gravity, 0))
self.body1 = ode.Body(self.world)
self.body2 = ode.Body(self.world)
self.create_basebox(self.body1, (0., 0., 0.), self.mass_cart, size_box)
self.create_link(self.body2, (0., size_pole[0] / 2, 0.),
self.mass_pole, 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.j1.setParam(ode.ParamLoStop, -10)
self.j1.setParam(ode.ParamHiStop, 10)
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.j2.setParam(ode.ParamLoStop, -np.pi)
self.j2.setParam(ode.ParamHiStop, np.pi)
def __init__(self):
self.world = ode.World()
self.world.setGravity( Vector3(0,-9.81,0) )
self.space = ode.Space()
self.boxes = {}
self.contactgroup = ode.JointGroup()
self.redis = redis.StrictRedis()
self.redis_pubsub = redis.StrictRedis()
self.channel = self.redis_pubsub.pubsub()
self.channel.subscribe('phys')
self.redis_fd = self.channel.connection._sock.fileno()
self.floor = ode.GeomPlane(self.space, (0,1,0), 0)
def __init__(self):
self.dt=.005
self.viewer = None
self.viewerSize = 500
self.spaceSize = 6.4
self.resolution = self.viewerSize/self.spaceSize
self.init_rod_template()
self.seed()
self.world = ode.World()
#self.world.setGravity((0,-9.81,0))
self.world.setGravity((0,0,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_link(self.body1,(0.5,0,0))
self.create_link(self.body2,(1.5,0,0))
self.create_link(self.body3,(2.5,0,0))
self.space = ode.Space()
self.body4_col = ode.GeomSphere(self.space,radius=0.1)
self.create_ee(self.body4,(3,0,0),self.body4_col)
# Connect body1 with the static environment
self.j1 = ode.HingeJoint(self.world)
self.j1.attach(self.body1, ode.environment)
self.j1.setAnchor( (0,0,0) )
self.j1.setAxis( (0,0,1) )
self.j1.setFeedback(1)
# Connect body2 with body1
self.j2 = ode.HingeJoint(self.world)
self.j2.attach(self.body1, self.body2)
self.j2.setAnchor( (1,0,0) )
self.j2.setAxis( (0,0,-1) )
self.j2.setFeedback(1)
#connect body3 with body2
self.j3 = ode.HingeJoint(self.world)
self.j3.attach(self.body2, self.body3)
self.j3.setAnchor( (2,0,0) )
self.j3.setAxis( (0,0,-1) )
self.j3.setFeedback(1)
#connect end effector
self.j4 = ode.FixedJoint(self.world)
self.j4.attach(self.body3,self.body4)
self.j4.setFixed()
self.controlMode = "POS"
self.targetPos = self.rand_target()
self.targetTime = 0
self.P_gains = np.array([1000,1000,1000])
self.D_gains = np.array([70,50,20])
def __init__(self):
viz.EventClass.__init__(self)
# Keep track of physnodes in here
self.physNodes_phys = []
# This will be turned to TRUE when a collision has been detected
self.collisionDetected = False
# ODE initialization steps
self.world = ode.World()
print(
'physEnv.init(): FIX: Grav hardcoded at 9.8. Should accept gravity as a parameter, or include a function to change gravity'
)
self.world.setGravity([0, -9.8, 0])
#self.world.setCFM(0.00001)
#self.world.setERP(0.05)
self.world.setCFM(0.00001)
self.world.setERP(0.1)
#self.world.setContactSurfaceLayer(0.001)
##bounce_vel is the minimum incoming velocity to cause a bounce
# Collision space where geoms live and collisions are simulated
# 0 for a 'simple' space (faster and less accurate), 1 for a hash space
self.space = ode.Space(1)
self.minBounceVel = .2 # min vel to cause a bounce
#### A better description:
##Spaces are containers for geom objects that are the actual objects tested for collision.
##For the collision detection a Space is the same as the World for the dynamics simulation, and a geom object corresponds to a body object.
##For the pure dynamics simulation the actual shape of an object doesn't matter, you only have to know its mass properties.
##However, to do collision detection you need to know what an object actually looks like, and this is what's the difference between a body and a geom.
# A joint group for the contact joints that are generated whenever two bodies collide
self.jointGroup = ode.JointGroup()
self.collisionList_idx = []
self.contactJoints_idx = []
self.contactObjects_idx = []
# A list of non-collision joints, such as fixed joints, etc
self.joints_jIdx = []
############################################################################################
############################################################################################
## Contact/collision functions
vizact.onupdate(viz.PRIORITY_PHYSICS, self.stepPhysics)
def InitPhysics():
world = ode.World()
world.setGravity((0, 60, 0))
world.setQuickStepNumIterations(100)
world.setERP(0.1)
space = ode.Space()
#floor = ode.GeomPlane(space,(0,-1,0),-600)
contactgroup = ode.JointGroup()
return world, space, contactgroup
class PhysicsEngine(object):
world = ode.World()
space = ode.Space()
contactgroup = ode.JointGroup()
@classmethod
def start(cls, gravity=(0, -9.8, 0), erp=.8, cfm=1e-5):
cls.world = ode.World()
cls.world.setGravity(gravity)
cls.world.setERP(erp)
cls.world.setCFM(cfm)
cls.space = ode.Space()
cls.contactgroup = ode.JointGroup()
@classmethod
def step(cls, step):
cls.space.collide(None, cls._collidecallback)
cls.world.step(step)
cls.contactgroup.empty()
@classmethod
def _collidecallback(cls, args, geom1, geom2):
go1 = geom1.gameobject
go2 = geom2.gameobject
go1.oncollision(go2)
go2.oncollision(go1)
for contact in ode.collide(geom1, geom2):
contact.setBounce(0)
contact.setMu(10000)
j = ode.ContactJoint(cls.world, cls.contactgroup, contact)
j.attach(geom1.getBody(), geom2.getBody())
@classmethod
def createbody(cls):
return ode.Body(cls.world)
@classmethod
def createmass(cls):
return ode.Mass()
@classmethod
def creategeom(cls, geomtype, args):
geomtype = "Geom" + geomtype
try:
return ode.__dict__[geomtype](cls.space, *args)
except:
raise PhysicsEngineError("Invalid Geom type: %s "
"is not defined in `ode'" % geomtype)
def __init__(self):
self.objectList = []
self.performance = 0
ode.World.__init__(self)
self.coach = Coach()
self.camera = Camera(0.0, 14.0, 19.5, -30.0, 0.0)
self.objectList.append(self.camera)
self.setGravity((0.0, -9.81, 0.0))
self.contactgroup = ode.JointGroup()
self.space = ode.Space()
self.accum = 0.0
self.time0 = 0
self.startTime = 0
def clear(self):
'''
deletes all the ode geometry in the scene
'''
if self._DISPLAY_INITIALIZED:
self._display.EraseAll()
for i in self._dynamic_shapes:
if i._collision_geometry is None:
continue
else:
if self._space.query(i._collision_geometry):
self._space.remove(i._collision_geometry)
self._dynamic_shapes = []
self._joints = []
self._space = ode.Space()
self._contactgroup = ode.JointGroup()
def __init__(self):
self.objectList = []
self.performance = 0
ode.World.__init__(self)
self.setGravity((0.0, -9.81 / 2, 0.0))
self.space = ode.Space()
self.contactgroup = ode.JointGroup()
self.time0 = 0
#here are menus: mainmenu, ingame
self.entryList = [
MainEntries(),
InGameEntries(),
RulesEntries(),
AboutEntries()
]
self.entryIndex = EntryIndex()
self.font_aa = GLFont(r"DATA\\FNT\\bold_aa.glf")
self.camera = Camera(0.0, 24.0, 0.0, -30.0, 0.0)
def createWorld(self, config):
self.world = ode.World()
self.space = ode.Space()
self.contactgroup = ode.JointGroup()
if config == None:
config = ypc.WorldConfig()
self.timeStep = config.timeStep
self.world.setGravity(config.gravity)
if config.planeHeight != None:
self.plane = ode.GeomPlane(self.space, (0, 1, 0),
config.planeHeight)
self.world.setContactSurfaceLayer(config.ContactSurfaceLayer)
self.world.setContactMaxCorrectingVel(config.ContactMaxCorrectingVel)
self.world.setERP(config.ERP)
self.world.setCFM(config.CFM)
def __init__(self, dt=(1 / 45.)):
'''This section incorporates PyODE to define methods to create and draw three objects: a sphere, box, or cylinder.
Some code borrowed from http://sourceforge.net/p/pyode/mailman/message/19674697/.
'''
self.dt = dt
self.entities = []
# Create a world object
self.ode_world = ode.World()
self.ode_world.setGravity(np.array([0, 0, Constants.g]))
self.ode_world.setAngularDamping(0.2)
# self.ode_world.setLinearDamping(0.2)
self.ode_world.setERP(0.8)
self.ode_world.setCFM(1E-5)
# Create a space object
self.space = ode.Space()
self.contact_group = ode.JointGroup()
self.ode_world.step(self.dt)
def __init__(self,num):
# Create a world object
self.world = ode.World()
self.world.setGravity( (0,-self.g,0) )
self.world.setERP(0.8)
self.world.setCFM(1.e-5)
self.space = ode.Space()
self.contactgroup = ode.JointGroup()
# create objects
self.anchor = Sphere(self.world,self.space,-2,1.e30,0.001,False)
self.pendulum = Sphere(self.world,self.space,-1,self.d,self.r,True)
self.joint = ode.BallJoint(self.world)
self.joint.attach(self.anchor.body,self.pendulum.body)
self.sat = [Sphere(self.world,self.space,s,self.ds,self.rs,False) for s in range(num)]
self.walls = (
ode.GeomPlane(self.space, ( 1, 0, 0), -(self.l+self.r)*1.1),
ode.GeomPlane(self.space, ( 0, 1, 0), -(self.l+self.r)*1.1),
ode.GeomPlane(self.space, ( 0, 0, 1), -(self.l+self.r)*1.1),
ode.GeomPlane(self.space, (-1, 0, 0), -(self.l+self.r)*1.1),
ode.GeomPlane(self.space, ( 0,-1, 0), -self.l*0.1),
ode.GeomPlane(self.space, ( 0, 0,-1), -(self.l+self.r)*1.1)
)
# remember bodies
self.geoms = {}
self.geoms[self.anchor.geom] = -2
self.geoms[self.pendulum.geom] = -1
for s in range(num):
self.geoms[self.sat[s].geom] = s
for s in range(len(self.walls)):
self.geoms[self.walls[s]] = 1000+s
self.reset_1()
