def construct(self):
self.geom1 = ode.GeomSphere(objects.space, self.radius)
self.geom2 = ode.GeomSphere(objects.space, self.radius)
self.geom1.setPosition((self.pos[0], self.pos[1], 0))
self.geom2.setPosition((self.pos[0] + self.offset, self.pos[1], 0))
self.geom1.data = weakref.ref(self)
self.geom2.data = weakref.ref(self)
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 create_sphere(self, key, density, radius, pos):
"""Create a sphere and its corresponding geom.
Arguments:
@param key: number id to assign to the sphere
@type key: int
@param density: density of a sphere
@type density: float
@param radius: radius of a sphere
@type radius: float
@param pos: list of floats of the position for the sphere
@type pos: [float,float,float]
"""
# Create body
body = ode.Body(self.world)
M = ode.Mass()
M.setSphere(density, radius)
body.setMass(M)
# Set the position of the body.
body.setPosition((pos))
# Set parameters for drawing the body
body.shape = "sphere"
body.radius = radius
# Create a sphere geom for collision detection
geom = ode.GeomSphere(self.space, radius)
geom.setBody(body)
# Append to the manager lists.
self.bodies[key] = body
self.geoms[key] = geom
def add_sphere(radius, density, position, quaternion=None, color=0x772277):
"""Create a sphere object, inlcuding visualization parameters.
"""
# Create rigid body and set position and rotation
sphere = ode.Body(world)
sphere.setPosition(position)
if quaternion is not None:
sphere.setQuaternion(quaternion)
# Set the mass based on density
mass = ode.Mass()
mass.setSphere(density, radius)
sphere.setMass(mass)
# Create the collision geometry
geom = ode.GeomSphere(space, radius)
geom.setBody(sphere)
geoms.append(geom)
# Create the json primitive
vis_prim = create_json_primitive("sphere", radius, color)
json_object["primitives"].append(vis_prim)
bodies.append(sphere)
return sphere
def update(self, t):
self.r1 = self.speed * (t - self.startT)
self.r2 = self.speed * (t - self.endT)
if self.geom1 is not None:
self.geom1.setRadius(self.r1)
else:
self.geom1 = ode.GeomSphere(self.fieldSpace, self.r1)
self.geom1.setPosition(self.center)
self.geom1.parent = self
if self.geom2 is not None:
self.geom2.setRadius(self.r2)
else:
self.geom2 = ode.GeomSphere(self.fieldSpace, self.r2)
self.geom2.setPosition(self.center)
self.geom2.parent = self
def construct(self):
self.body = ode.Body(world)
M = ode.Mass()
M.setSphereTotal(self.mass, self.rad)
self.body.setMass(M)
self.body.setPosition((self.x, self.y, 0))
self.geom = ode.GeomSphere(space, self.rad)
self.geom.setBody(self.body)
ODEThing.construct(self)
def _create_geom(self):
# find vertex furthest from centroid
max_rad = max([v.dot(v).item() for v in self.verts])
max_rad = math.sqrt(max_rad)
# XXX Using sphere with largest vertex ray for broadphase for now
self.geom = ode.GeomSphere(None, max_rad + self.eps.item())
self.geom.setPosition(torch.cat([self.pos, self.pos.new_zeros(1)]))
self.geom.no_contact = set()
def _create_geom(self):
# find vertex furthest from centroid
max_rad = max([v.dot(v).data[0] for v in self.verts])
max_rad = math.sqrt(max_rad)
# XXX Using sphere with largest vertex ray for broadphase for now
self.geom = ode.GeomSphere(None, max_rad + self.eps.data[0])
self.geom.setPosition(torch.cat([self.pos.data, Tensor(1).zero_()]))
self.geom.no_collision = set()
def _create_ball(world, ball_mass, ball_radius, space=None):
mass = ode.Mass()
mass.setSphereTotal(ball_mass, ball_radius)
body = ode.Body(world)
body.setMass(mass)
body.shape = "sphere"
body.boxsize = (2 * ball_radius, 2 * ball_radius, 2 * ball_radius)
geom = ode.GeomSphere(space=space, radius=ball_radius)
geom.setBody(body)
return body, geom
def DefineSphere(self, Density, Radius):
"""Define this element as an ode Sphere."""
if self._hasGeom:
self._geom = ode.GeomSphere(None, Radius)
DisplayElement.SetDisplayObject(self, visual.sphere(radius=Radius))
self._mass = ode.Mass()
self._mass.setSphere(Density, Radius)
return self
def create_sphere(self, density, radius):
body = ode.Body(self.world)
M = ode.Mass()
M.setSphere(density, radius)
body.setMass(M)
geom = ode.GeomSphere(self.space[0], radius=radius)
geom.setBody(body)
return body, geom
def create_sphere(self,
key,
density,
radius,
pos,
active_surfaces={
'x': 1,
'y': 1,
'z': 1,
'-x': 1,
'-y': 1,
'-z': 1
}):
"""Create a sphere and its corresponding geom.
Arguments:
@param key: number id to assign to the sphere
@type key: int
@param density: density of a sphere
@type density: float
@param radius: radius of a sphere
@type radius: float
@param pos: list of floats of the position for the sphere
@type pos: [float,float,float]
"""
# Auto label the joint key or not.
key = len(self.bodies) if key == -1 else key
# Create body
body = ode.Body(self.world)
M = ode.Mass()
M.setSphere(density, radius)
body.setMass(M)
# Set the position of the body.
body.setPosition((pos))
# Set parameters for drawing the body
body.type = "sphere"
body.shape = "sphere"
body.radius = radius
# Create a sphere geom for collision detection
geom = ode.GeomSphere(self.space, radius)
geom.setBody(body)
# Append to the manager lists.
self.bodies[key] = body
self.geoms[key] = geom
# Create surfaces if doing fluids
if (self.fluid_dynamics):
self.create_surfaces(key, 1., active_surfaces)
return key
def DefineSphereTotal(self, TotalMass, Radius):
"""Define this element as an ode SphereTotal."""
if self._hasGeom:
self._geom = ode.GeomSphere(None, Radius)
DisplayElement.SetDisplayObject(self, visual.sphere(radius=Radius))
self._mass = ode.Mass()
self._mass.setSphereTotal(TotalMass, Radius)
self._mass.mass = TotalMass # Bug workaround?
return self
def rand_target(self):
pos = (np.random.rand(3,1)-0.5)*4
self.invalpos = (np.random.rand(3,1)-0.5)
self.invalpos += np.sign(self.invalpos)* 0.2
self.invalpos += pos
self.targetGeom = ode.GeomSphere(self.space, radius=0.1)
self.targetGeom.setPosition((pos[0],pos[1],pos[2]))
spikes = [3,4,5,7,9]
temp= np.random.randint(3)
self.targetParams = [0.12,spikes[temp]]
self.invalidParams= [0.08,1,0.10*np.random.rand()+0.4]
self.target = rendering.make_valid_target(self.targetParams[0],self.targetParams[1])
self.targetTrans = rendering.Transform()
self.target.add_attr(self.targetTrans)
self.invalid_target = rendering.make_invalid_target(self.invalidParams[0],self.invalidParams[1],self.invalidParams[2])
self.invalidTargetTrans = rendering.Transform()
self.invalid_target.add_attr(self.invalidTargetTrans)
self.intargetGeom = ode.GeomSphere(self.space, radius=0.1)
self.intargetGeom.setPosition((self.invalpos[0],self.invalpos[1],self.invalpos[2]))
return pos
def createSphere(self, sx, sz):
sphere = ode.Body(self.world)
mSphere = ode.Mass()
mSphere.setSphere(PERSONDENSITY, SPHERERADIUS)
sphere.setMass(mSphere)
geomSphere = ode.GeomSphere(self.space, SPHERERADIUS)
geomSphere.setBody(sphere)
sphere.setPosition((sx, SPHERERADIUS, sz))
self.viz.addGeom(geomSphere)
self.viz.GetProperty(sphere).SetColor(PERSONCOLOR)
return sphere, geomSphere
def run_simulation(self):
if self.trace:
faulthandler.enable()
sys.settrace(module.trace_func)
# dynamics world
world = ode.World()
world.setGravity((0, -10, 0)) # heightmaps consider Y as up
# collision space
space = ode.SimpleSpace() # SimpleSpace is slow-ish
# contact group
contactgroup = ode.JointGroup()
# mass
M = ode.Mass()
M.setSphere(2500, 1)
M.mass = 1.0
# dynamic body
body = ode.Body(world)
body.setMass(M)
body.setPosition((0, 260, 0))
body.setMovedCallback(self.body_callback)
# collision geom
geom = ode.GeomSphere(space, 1)
geom.setBody(body)
# heightmap
h_data = ode.HeightfieldData()
im = Image.open(sys.path[0] + "/tutorial_heightmap.png", "r")
width, height = im.size
pixel_values = list(
im.getdata(0)
) # 0 is the index of the Red channel in RGB / RGBA images - returns a list of longs for some reason tho
height_data = np.array(pixel_values)
height_data = height_data.astype(
np.ubyte) # convert array to unsigned bytes as ODE expects so
height_data = np.ascontiguousarray(
height_data
) # convert to a contiguous array so C code can read it and store data in variable to prevent garbage collection
h_data.buildByte(height_data, False, width, height, width, height, 1,
0, 1, True)
h_geom = ode.GeomHeightfield(data=h_data, space=space)
# run simulation
total_time = 0.0
dt = 0.04
while total_time < 4.5:
print(total_time)
space.collide((world, contactgroup),
self.collision_callback) # collision detection
world.quickStep(dt) # dynamics step
contactgroup.empty()
total_time += dt
def __init__(self, sim, density=1000, radius=1.0, label=None, **kw):
body = ode.Body(sim.world)
M = ode.Mass()
M.setSphere(density, radius)
self.size = (radius, radius, radius) # ?
body.setMass(M)
geom = ode.GeomSphere(sim.space, radius)
geom.setBody(body)
BodyItem.__init__(self, sim, body, geom, label, **kw)
def __init__(self,world,space,index,density,radius,grav):
Object.__init__(self,world,index,grav)
m = ode.Mass()
m.setSphere(density,radius)
self.body.setMass(m)
# Create a geom for collision detection
self.geom = ode.GeomSphere(space, radius=radius)
self.geom.setBody(self.body)
self.m = spheremass(density,radius)
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, u, par, level):
Movable.__init__(self, u)
RawGLObject.__init__(self, u)
mass = ode.Mass()
r = 0.05
density = 100
mass.setSphere(
density, r
) #NB segment is a sphere mass -- so does not need to rotate. the bending is done by joints BETWEEN the masses
self.body = ode.Body(self.u.world)
self.body.setMass(mass)
self.level = level
x, y, z = par.getBodyPositionForChildSeg()
seg = 0.4
if level == 1:
x += 0.15 #65
z += 0.15
elif level == -1:
x += 0.15
z -= 0.15
elif level > 1:
x += seg
z += seg
elif level < -1:
x += seg
z -= seg
#x+=x_offset
#x=x+SEG_LENGTH #TODO fix properly
#z=z+z_offset
self.body.setPosition((x, y, z))
self.geom = ode.GeomSphere(u.segSpace, r) #SEPARATE SPACE!
self.geom.setBody(self.body)
self.sg.addChild(self.makeSceneGraph(r))
self.joint = ode.HingeJoint(
self.u.world
) #easier to use horiz/vertical pairs of hinges rather than balls; as have getAngle methods
self.joint.attach(par.body, self.body)
self.joint.setAnchor(par.body.getPosition())
self.joint.setAxis((0, 1, 0))
if abs(level) == 1: #follicle has different properties
self.k = 1.0
self.gamma = 0.5
else:
self.k = 0.1 #TODO we really need two different classes of sim: one of rmacro navigation excersizes, and one for very fine detailed vibrtation sims.
self.gamma = 0.05
self.par = par #needed for link graphics
def __init__(self, world, space, position, density, radius):
self.body = ode.Body(world)
self.body.setPosition(position)
M = ode.Mass()
M.setSphere(density, radius)
self.body.setMass(M)
self.radius = radius
self._linear_damping_coeff *= radius
self._rotational_damping_coeff *= radius
# Create a Sphere geom for collision detection
self.geom = ode.GeomSphere(space, radius)
self.geom.setBody(self.body)
def create(self):
self.body = ode.Body(self.world)
mass = ode.Mass()
mass.setCylinder(100, 2, 0.4, 1.8)
mass.setSphere(100, 4)
geom = ode.GeomCylinder(self.space, 0.4, 1.8)
geom = ode.GeomSphere(self.space, 4)
geom.setBody(self.body)
x, y, z = self.position
self.body.setRotation((1, 0, 0, 0, 0, -1, 0, 1, 0))
self.body.setPosition((x, y + 0.9, z))
self.viz.addGeom(geom)
self.viz.GetProperty(self.body).SetColor(self.personColor)
self.setLinearVelocity()
def DefineSphere(self, ElementKey, Body, Density, Radius):
"""Define this element as an ode Sphere."""
if self._hasGeom:
self._geom = ode.GeomSphere(_bigSpace, Radius)
DisplayElement.SetDisplayObject(
self, ivisual.sphere(frame=Body._myFrame, radius=Radius))
self._mass = ode.Mass()
self._mass.setSphere(Density, Radius)
self.AddGDMElement(Body, ElementKey)
return self
def __init__(self, id, universe, geometry="sphere", *args, **kwargs):
self.id = id
self.universe = universe
self.body = ode.Body(universe.world)
if geometry == "sphere":
self.geom = ode.GeomSphere(universe.space)
else:
# TODO: Add other geometries
raise ValueError("geometry must be 'sphere'")
self.geom.setBody(self.body)
all_entities[self.id] = self
def use_sphere(self):
''' Connect a ode.GeomSphere to the body
The sphere radius is computed as the mean of the bounding box dimensions. As a consequence,
if the TopoDS_Shape is a sphere, then the radius of the ode.GeomSphere will be the radius
of the sphere
'''
bbox = get_boundingbox(self._shape, EPSILON)
xmin, ymin, zmin, xmax, ymax, zmax = bbox.Get()
dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
r = (
dx + dy + dz
) / 6. # /3 gives the mean diameter, so divide by 2 for the radius
self._collision_geometry = ode.GeomSphere(self._space, radius=r)
self._collision_geometry.setBody(self)
def _create_sphere(self, space, density, radius):
"""Create a sphere body and its corresponding geom."""
# Create body and mass
body = ode.Body(self.world)
M = ode.Mass()
M.setSphere(density, radius)
body.setMass(M)
body.name = None
# Create a sphere geom for collision detection
geom = ode.GeomSphere(space, radius)
geom.setBody(body)
geom.name = None
return (body, geom)
def __init__(self, space, world, radius, mass, color=None):
self._radius = radius
self._color = color
self._odemass = ode.Mass()
self._odemass.setSphereTotal(0.00001, radius)
self._odemass.adjust(mass)
self._odebody = ode.Body(world)
self._odebody.setMass(self._odemass)
self._odegeom = ode.GeomSphere(space, radius)
self._odegeom.setBody(self._odebody)
# Observation
self._neighbors = 2
self._obs_dim = 2 * self._neighbors + 2 # Acceleration of n nearest neighbors + Velocity of object
def DefineSphereTotal(self, ElementKey, Body, TotalMass, Radius):
"""Define this element as an ode SphereTotal."""
if self._hasGeom:
self._geom = ode.GeomSphere(_bigSpace, Radius)
DisplayElement.SetDisplayObject(
self, ivisual.sphere(frame=Body._myFrame, radius=Radius))
self._mass = ode.Mass()
self._mass.setSphereTotal(TotalMass, Radius)
self._mass.mass = TotalMass # Bug workaround?
self.AddGDMElement(Body, ElementKey)
return self
def create_ball(world, space, density, radius):
"""create a ball body and its corresponding geom."""
#create the body (the physical volume to simulate)
body = ode.Body(world)
M = ode.Mass()
M.setSphere(density, radius)
body.setMass(M)
# set parameters for drawing the body
body.shape = "ball"
body.radius = radius
# create a sphere geom for collision detection
geom = ode.GeomSphere(space, radius=body.radius)
geom.setBody(body)
return body
def create_sphere(world, space, density, radius):
"""Create a box body and its corresponding geom."""
# Create body
body = ode.Body(world)
M = ode.Mass()
M.setSphereTotal(density, radius)
body.setMass(M)
# Set parameters for drawing the body
body.shape = "sphere"
body.radius = radius
# Create a box geom for collision detection
geom = ode.GeomSphere(space, radius=body.radius)
geom.setBody(body)
return body, geom
