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 buildObjects():
world = ode.World()
world.setGravity((0, -9.81, 0))
body1 = ode.Body(world)
M = ode.Mass()
M.setSphere(2500, 0.05)
body1.setMass(M)
body1.setPosition((1, 2, 0))
body2 = ode.Body(world)
M = ode.Mass()
M.setSphere(2500, 0.05)
body2.setMass(M)
body2.setPosition((2, 2, 0))
j1 = ode.BallJoint(world)
j1.attach(body1, ode.environment)
j1.setAnchor((0, 2, 0))
#j1 = ode.HingeJoint(world)
#j1.attach(body1, ode.environment)
#j1.setAnchor( (0,2,0) )
#j1.setAxis( (0,0,1) )
#j1.setParam(ode.ParamVel, 3)
#j1.setParam(ode.ParamFMax, 22)
j2 = ode.BallJoint(world)
j2.attach(body1, body2)
j2.setAnchor((1, 2, 0))
return world, body1, body2, j1, j2
def loadModel(self, filename):
mesh = loadObj(filename)
body = ode.Body(self.world)
self.model = ode.GeomTriMesh(mesh, self.space[0])
self.model.setBody(body)
# rotate so that z becomes top
self.addGeom(self.model, "model")
self.model.setQuaternion(
(0.7071067811865476, 0.7071067811865475, 0, 0))
body = ode.Body(self.world)
ray = ode.GeomRay(self.space[0], 10000)
ray.setBody(body)
ray.set((0, 0, 1), (0, 0, 100000))
self.addGeom(ray, "XXXXXXX")
self.GetProperty(ray).SetColor(1, 0, 0)
self.GetProperty(ray).SetLineWidth(30)
print(self.GetProperty(ray))
self.update()
def __init__(self, world, config):
"""
Create articulated rigid body fly object for dynamic simulation.
The fly's body is initialized so that it's center of mass is
(0,0,0) in world coordinates.
Inputs:
world = ODE simulation world object
config = simulation configuration dictionary
"""
# Create fly body
self.body = ode.Body(world)
# Create wings
self.r_wing = ode.Body(world)
self.l_wing = ode.Body(world)
# Create wing joints
self.r_joint = ode.BallJoint(world)
self.l_joint = ode.BallJoint(world)
# Create angular motors
self.r_amotor = ode.AMotor(world)
self.l_amotor = ode.AMotor(world)
# Set body according to configuration
self.set2Config(config)
def create_module(sim, parent_pos, lower_offset, upper_offset, rot):
"""
Spawns a CKBot module at the upper and lower body positions and specified rotation matrix.
"""
# Find the absolute positions of the module given the parent's position.
lower_pos = (parent_pos[0] + lower_offset[0],
parent_pos[1] + lower_offset[1],
parent_pos[2] + lower_offset[2])
upper_pos = (parent_pos[0] + upper_offset[0],
parent_pos[1] + upper_offset[1],
parent_pos[2] + upper_offset[2])
hinge_pos = (0.5 * (lower_pos[0] + upper_pos[0]),
0.5 * (lower_pos[1] + upper_pos[1]),
0.5 * (lower_pos[2] + upper_pos[2]))
# Create the lower piece of the module.
lowerbody = ode.Body(sim.world)
lowerjoint = ode.GeomBox(space=sim.space,
lengths=(sim.cubesize, sim.cubesize * 0.5,
sim.cubesize))
lowerjoint.setBody(lowerbody)
lowerjoint.setPosition(lower_pos)
lowerjoint.setRotation(rot)
lowerM = ode.Mass()
lowerM.setBox(sim.cubemass, sim.cubesize, sim.cubesize * 0.5, sim.cubesize)
lowerbody.setMass(lowerM)
# Create the upper piece of the module.
upperbody = ode.Body(sim.world)
upperjoint = ode.GeomBox(space=sim.space,
lengths=(sim.cubesize, sim.cubesize * 0.5,
sim.cubesize))
upperjoint.setBody(upperbody)
upperjoint.setPosition(upper_pos)
upperjoint.setRotation(rot)
upperM = ode.Mass()
upperM.setBox(sim.cubemass, sim.cubesize, sim.cubesize * 0.5, sim.cubesize)
upperbody.setMass(upperM)
# Create the hinge of the module.
hinge = ode.HingeJoint(sim.world)
hinge.attach(lowerbody, upperbody)
hinge.setAnchor(hinge_pos)
hinge.setAxis(rotate((1, 0, 0), rot))
hinge.setParam(ode.ParamLoStop, sim.hingeminangle)
hinge.setParam(ode.ParamHiStop, sim.hingemaxangle)
hinge.setParam(ode.ParamFMax, sim.hingemaxforce)
# Append all these new pointers to the simulator class.
sim.lowerbody.append(lowerbody)
sim.lowerjoint.append(lowerjoint)
sim.lowerM.append(lowerM)
sim.upperbody.append(upperbody)
sim.upperjoint.append(upperjoint)
sim.upperM.append(upperM)
sim.hinge.append(hinge)
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.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 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 createCapsule(self, mass=100, length=1, radius=1, pos=(0, 0, 0)):
"""Creates a capsule body and corresponding geom."""
body = ode.Body(self.world)
m = ode.Mass()
m.setCappedCylinderTotal(mass, 3, radius, length)
body.setMass(m)
# set parameters for drawing the body
body.shape = "capsule"
body.length = length
body.radius = radius
# create a capsule geom for collision detection
geom = ode.GeomCCylinder(self.space, radius, length)
geom.setBody(body)
# create a reference to the body geom in case the user does not want to keep track of it.
body.geom = geom
body.color = (128, 0, 0, 255)
# This is the constant offset for aligning with the physics element
# This is a 4x4 opengl style matrix, expressing an offset and a rotation
offset = (0, 0, 0)
rot = calcRotMatrix((0, 0, 1), 0)
body.glObjOffset = makeOpenGLMatrix(rot, offset, (1.0, 1.0, 1.0))
body.setPosition(pos)
self.bodies.append(body)
return body, geom
def process_cutter_movement(self, location_start, location_end):
# TODO: fix this workaround in the cutters shape defintions (or in ODE?)
# for now we may only move from low x/y values to higher x/y values
if (location_start.x > location_end.x) \
or (location_start.y > location_end.y):
location_start, location_end = location_end, location_start
cutter_body = ode.Body(self.world)
cutter_shape, cutter_position_func = self.cutter.get_shape("ODE")
self.space.add(cutter_shape)
cutter_shape.space = self.space
cutter_shape.setBody(cutter_body)
cutter_position_func(location_start.x, location_start.y,
location_start.z)
cutter_shape.extend_shape(location_end.x - location_start.x,
location_end.y - location_start.y,
location_end.z - location_start.z)
aabb = cutter_shape.getAABB()
cutter_height = aabb[5] - aabb[4]
# add a ray along the drill to work around an ODE bug in v0.11.1
# http://sourceforge.net/tracker/index.php?func=detail&aid=2973876&group_id=24884&atid=382799
currx, curry, currz = cutter_shape.getPosition()
ray = ode.GeomRay(self.space, cutter_height)
ray.set((currx, curry, aabb[5]), (0.0, 0.0, -1.0))
# check for collisions
all_cutter_shapes = [cutter_shape] + cutter_shape.children + [ray]
def check_collision(item):
for one_cutter_shape in all_cutter_shapes:
if ode.collide(item, one_cutter_shape):
return True
return False
for index in range(len(self.boxes)):
if check_collision(self.boxes[index]):
self.shrink_box_avoiding_collision(index, check_collision)
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, u, par, z_offset):
Movable.__init__(self, u)
self.par = par #needed for link graphics
mass = ode.Mass()
axis = 0
radius = 0.3
depth = 0.1
mass.setCylinder(100, axis, radius,
depth) #gets rotated by any geom rots
x, y, z = self.par.body.getPosition()
z += z_offset
self.body = ode.Body(self.u.world)
self.body.setMass(mass)
self.body.setPosition((x, y, z))
self.geom = ode.GeomCylinder(u.segSpace, radius,
depth) #SEPARATE SPACE!
self.geom.setBody(self.body)
self.sg.addChild(self.makeSceneGraph(radius, depth))
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((x, y, z))
self.joint.setAxis((0, 0, 1))
def create(self):
self.body = ode.Body(self.world)
mass = ode.Mass()
mass.setBox(ROADDENSITY, ROADLENGTH, ROADHEIGHT, self.size)
self.body.setMass(mass)
self.geom = ode.GeomBox(self.space,
(ROADLENGTH, ROADHEIGHT, self.size))
self.geom.setBody(self.body)
self.geom.setPosition((0, 0, self.zPos))
self.viz.addGeom(self.geom)
self.viz.GetProperty(self.body).SetColor(ROADCOLOR)
p = []
for n in range(int(self.numLanes)):
p.append(
(-100, self.yPos,
self.zPos + (self.numLanes / 2 - n) * ROADSIZE, 1500,
self.yPos, self.zPos + (self.numLanes / 2 - n) * ROADSIZE, 1))
p.append(
(-100, self.yPos, self.zPos + self.numLanes / 2 * ROADSIZE, 1500,
self.yPos, self.zPos + self.numLanes / 2 * ROADSIZE, 0))
p.append(
(-100, self.yPos, self.zPos - self.numLanes / 2 * ROADSIZE, 1500,
self.yPos, self.zPos - self.numLanes / 2 * ROADSIZE, 0))
if (self.numLanes % 2 == 0):
if (self.direction):
p.append((-100, self.yPos, self.zPos, 1500, self.yPos,
self.zPos, 0))
else:
p.append((-100, self.yPos, self.zPos, 1500, self.yPos,
self.zPos, 0))
self.viz.drawLines(p)
def addBody(self, type, p1, p2, radius):
p1 = add3(p1, self.offset)
p2 = add3(p2, self.offset)
length = dist3(p1, p2)
body = ode.Body(self.world)
m = ode.Mass()
if type == 'horizontal':
m.setCylinder(self.density, 3, radius, length)
body.setMass(m)
body.shape = "capsule"
body.length = length
body.radius = radius
geom = ode.GeomCylinder(self.space, radius, length)
geom.setBody(body)
elif type == 'vertical':
m.setBox(self.density, radius, radius, length)
body.setMass(m)
body.shape = "box"
body.length = length
body.radius = radius
geom = ode.GeomBox(self.space, lengths=(radius, radius, length))
geom.setBody(body)
elif type == 'base':
m.setBox(BASE_DENSITY, radius, radius, length)
body.setMass(m)
body.shape = "box"
body.length = length
body.radius = radius
geom = ode.GeomBox(self.space, lengths=(radius, radius, length))
geom.setBody(body)
# define body rotation automatically from body axis.
za = norm3(sub3(p2, p1))
if (abs(dot3(za, (1.0, 0.0, 0.0))) < 0.7): xa = (1.0, 0.0, 0.0)
else: xa = (0.0, 1.0, 0.0)
ya = cross(za, xa)
xa = norm3(cross(ya, za))
ya = cross(za, xa)
rot = (xa[0], ya[0], za[0], xa[1], ya[1], za[1], xa[2], ya[2], za[2])
body.setPosition(mul3(add3(p1, p2), 0.5))
body.setRotation(rot)
self.bodies.append(body)
self.geoms.append(geom)
self.totalMass += body.getMass().mass
return body
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 create_box(world, space, density, lx, ly, lz):
"""Create a box body and its corresponding geom."""
# Create body
body = ode.Body(world)
M = ode.Mass()
M.setBox(density, lx, ly, lz)
body.setMass(M)
# print body.getMass()
# Set parameters for drawing the body
body.shape = "box"
body.id = objcount + 1
body.attached = 0
body.attachedTo = []
body.facesActivated = []
body.facesActivatedTo = []
body.boxsize = (lx, ly, lz)
body.delayCounter = 0
body.state = 'a'
# Create a box geom for collision detection
geom = ode.GeomBox(space, lengths=body.boxsize)
geom.setBody(body)
return body, geom
def makePhysicsBody(self):
physicsWorld = self.environment.world
space = self.environment.space
mainBody = ode.Body(physicsWorld)
bodyMass = ode.Mass()
totalMass = self.bodyMass+4*self.motorMass
offset = self.armLength + float(self.bodyLength) / 2.0
#one arm
bodyMass.setBoxTotal(totalMass/2.0, offset, self.bodyHeight, self.motorRadius)
# next arm
armMass = ode.Mass()
armMass.setBoxTotal(totalMass/2.0, self.motorRadius, self.bodyHeight, offset)
bodyMass.add(armMass)
firstArmGeom = ode.GeomBox(space, (offset, self.bodyHeight, self.motorRadius))
firstArmGeom.setCategoryBits(1)
firstArmGeom.setCollideBits(1)
secondArmGeom = ode.GeomBox(space, (self.motorRadius, self.bodyHeight, offset))
secondArmGeom.setCategoryBits(1)
secondArmGeom.setCollideBits(1)
mainBody.setMass(bodyMass)
firstArmGeom.setBody(mainBody)
secondArmGeom.setBody(mainBody)
self.geomList = [firstArmGeom, secondArmGeom]
self.physicsBody = mainBody
def create_ray(self, key, pos, rot, length=5.0):
""" Create a ray geom.
Arguments:
key: key identifying the geom.
pos: position to start the ray from.
rot: list of pitch,roll,yaw in degrees (determines where it points)
"""
# Create body
body = ode.Body(self.world)
M = ode.Mass()
M.setBox(0.1, 0.1, 0.1, 0.1)
body.setMass(M)
# Set parameters for drawing the body
body.shape = "box"
body.boxsize = (0.1, 0.1, 0.1)
body.setPosition((pos[0], pos[1], pos[2]))
geom = ode.GeomRay(self.space, length)
geom.setPosition((pos))
#geom.setRotation(self.form_rotation(rot))
body.setRotation(self.form_rotation(rot))
geom.setBody(body)
# Set min_dist for sensor.
geom.min_dist = -1
# Append to the manager lists.
self.bodies[key] = body
self.geoms[key] = geom
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 self_create_box(self, density, lx, ly, lz):
"""Create a box body and its corresponding geom.
Arguments:
density - density of the given body
lx : x dimension
ly : y dimension
lz : z dimension
"""
# Create body
body = ode.Body(self.world)
M = ode.Mass()
M.setBox(density, lx, ly, lz)
body.setMass(M)
# Set parameters for drawing the body
body.shape = "box"
body.boxsize = (lx, ly, lz)
# Create a box geom for collision detection
geom = ode.GeomBox(self.space, lengths=body.boxsize)
geom.setBody(body)
return body, geom
def addBox(self, lx, ly, lz, offset, mass=None):
"""
Add a box (cuboid) with x, y and z lengths given by lx, ly and lz.
"""
body = ode.Body(self.sim.world)
# This is our own stupid shit
body.color = (128, 128, 40, 255)
m = ode.Mass()
if mass is None:
m.setBox(self.density, lx, ly, lz)
else:
m.setBoxTotal(mass, lx, ly, lz)
body.setMass(m)
# set parameters for drawing the body
body.shape = "box"
body.lx = lx
body.ly = ly
body.lz = lz
# create a capsule geom for collision detection
geom = ode.GeomBox(self.sim.space, (lx, ly, lz))
geom.setBody(body)
# TODO: allow feeding in a starting rotation
# For now all we need is position
body.setPosition(add3(offset, self.offset))
self.bodies.append(body)
self.geoms.append(geom)
self.totalMass += body.getMass().mass
return body
def addBody(self,
p1,
p2,
radius,
mass=None,
upVec=(0, 0, 1),
collide=True):
"""
Adds a capsule body between joint positions p1 and p2 and with given
radius to the ragdoll.
upVec is the vector pointing up. This resolves the ambiguity of
orienting a rotationally symetrical object between 2 points.
TODO: If p1->p2 || upVec, this will crash
"""
p1 = add3(p1, self.offset)
p2 = add3(p2, self.offset)
# cylinder length not including endcaps, make capsules overlap by half
# radius at joints
cyllen = dist3(p1, p2) - radius
body = ode.Body(self.sim.world)
body.color = (128, 128, 40, 255)
m = ode.Mass()
if mass is None:
m.setCappedCylinder(self.density, 3, radius, cyllen)
else:
m.setCappedCylinderTotal(mass, 3, radius, cyllen)
body.setMass(m)
# set parameters for drawing the body
body.shape = "capsule"
body.length = cyllen
body.radius = radius
# create a capsule geom for collision detection
if collide:
geom = ode.GeomCCylinder(self.sim.space, radius, cyllen)
geom.setBody(body)
self.geoms.append(geom)
# define body rotation automatically from body axis
za = norm3(sub3(p2, p1))
# if (abs(dot3(za, (1.0, 0.0, 0.0))) < 0.7): xa = (1.0, 0.0, 0.0)
# else: xa = (0.0, 1.0, 0.0)
xa = upVec
ya = cross(za, xa)
xa = norm3(cross(ya, za))
ya = cross(za, xa)
rot = (xa[0], ya[0], za[0], xa[1], ya[1], za[1], xa[2], ya[2], za[2])
body.setPosition(mul3(add3(p1, p2), 0.5))
body.setRotation(rot)
self.bodies.append(body)
self.totalMass += body.getMass().mass
return body
def addBody(self, p1, p2, radius):
# Adds a capsule body between joint positions p1 and p2 and with given
# radius to the ragdoll.
p1 = add3(p1, self.offset)
p2 = add3(p2, self.offset)
# cylinder length not including endcaps, make capsules overlap by half
# radius at joints
cyllen = dist3(p1, p2) + radius
"""
body = ode.Body(self.world)
m = ode.Mass()
m.setCappedCylinder(self.density, 3, radius, cyllen)
body.setMass(m)
# set parameters for drawing the body
body.shape = "capsule"
body.length = cyllen
body.radius = radius
# create a capsule geom for collision detection
geom = ode.GeomCCylinder(self.space, radius, cyllen)
geom.setBody(body)
"""
# Create body
body = ode.Body(self.world)
m = ode.Mass()
m.setBox(self.density, radius, radius, cyllen)
body.setMass(m)
# Set parameters for drawing the body
body.shape = "rectangle"
body.boxsize = (radius, radius, cyllen)
# Create a box geom for collision detection
geom = ode.GeomBox(self.space, lengths=body.boxsize)
geom.setBody(body)
# define body rotation automatically from body axis
za = norm3(sub3(p2, p1))
if (abs(dot3(za, (1.0, 0.0, 0.0))) < 0.7): xa = (1.0, 0.0, 0.0)
else: xa = (0.0, 1.0, 0.0)
ya = cross(za, xa)
xa = norm3(cross(ya, za))
ya = cross(za, xa)
rot = (xa[0], ya[0], za[0], xa[1], ya[1], za[1], xa[2], ya[2], za[2])
body.setPosition(mul3(add3(p1, p2), 0.5))
body.setRotation(rot)
self.bodies.append(body)
self.geoms.append(geom)
self.totalMass += body.getMass().mass
return body
def _add_geom(self, geom, position, append=True):
body = ode.Body(self._world)
body.setPosition(position)
body.setGravityMode(False)
geom.setBody(body)
if append:
self._obstacles.append(geom)
self._dirty = True
def create_ode_body(self, world, space, total_mass):
body = ode.Body(world)
body.setMass(self.primitive.create_ode_mass(total_mass))
body.shape = "box"
body.boxsize = tuple(self.primitive.lengths)
geom = self.primitive.create_ode_geom(space)
geom.setBody(body)
return body
def makePhysicsBody(self):
physicsWorld = self.environment.world
mainBody = ode.Body(physicsWorld)
mainBody.setKinematic()
self.physicsBody = mainBody
self.environment.addFieldObject('Vibration', self)
def makeBox(x, y):
body = ode.Body(self.world)
body.setPosition((x, y, -40))
geom = ode.GeomBox(self.space, lengths=(0.5, 0.5, 0.5))
geom.setBody(body)
self._geoms.append(geom)
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 construct(self):
self.body = ode.Body(world)
M = ode.Mass()
M.setBoxTotal(self.mass, self.L, self.H, 1)
self.body.setMass(M)
self.body.setPosition((self.x, self.y, 0))
self.geom = ode.GeomBox(space, (self.L, self.H, 10))
self.geom.setBody(self.body)
ODEThing.construct(self)
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
