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 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 extend_shape(diff_x, diff_y, diff_z):
reset_shape()
# see http://mathworld.wolfram.com/RotationMatrix.html
hypotenuse = sqrt(diff_x * diff_x + diff_y * diff_y)
# Some paths contain two identical points (e.g. a "touch" of the
# PushCutter). We don't need any extension for these.
if hypotenuse == 0:
return
cosinus = diff_x / hypotenuse
sinus = diff_y / hypotenuse
# create the cyclinder at the other end
geom_end_transform = ode.GeomTransform(geom.space)
geom_end_transform.setBody(geom.getBody())
geom_end = ode.GeomCapsule(None, radius, self.height)
geom_end.setPosition((diff_x, diff_y, diff_z + center_height))
geom_end_transform.setGeom(geom_end)
# create the block that connects the two cylinders at the end
rot_matrix_box = (cosinus, sinus, 0.0, -sinus, cosinus, 0.0,
0.0, 0.0, 1.0)
geom_connect_transform = ode.GeomTransform(geom.space)
geom_connect_transform.setBody(geom.getBody())
geom_connect = ode_physics.get_parallelepiped_geom(
(Point(-hypotenuse / 2, radius, -diff_z / 2),
Point(hypotenuse / 2, radius, diff_z / 2),
Point(hypotenuse / 2, -radius, diff_z / 2),
Point(-hypotenuse / 2, -radius, -diff_z / 2)),
(Point(-hypotenuse / 2, radius, self.height - diff_z / 2),
Point(hypotenuse / 2, radius, self.height + diff_z / 2),
Point(hypotenuse / 2, -radius, self.height + diff_z / 2),
Point(-hypotenuse / 2, -radius,
self.height - diff_z / 2)))
geom_connect.setRotation(rot_matrix_box)
geom_connect.setPosition((hypotenuse / 2, 0, radius))
geom_connect_transform.setGeom(geom_connect)
# Create a cylinder, that connects the two half spheres at the
# lower end of both drills.
geom_cyl_transform = ode.GeomTransform(geom.space)
geom_cyl_transform.setBody(geom.getBody())
hypotenuse_3d = Matrix.get_length((diff_x, diff_y, diff_z))
geom_cyl = ode.GeomCylinder(None, radius, hypotenuse_3d)
# rotate cylinder vector
cyl_original_vector = (0, 0, hypotenuse_3d)
cyl_destination_vector = (diff_x, diff_y, diff_z)
matrix = Matrix.get_rotation_matrix_from_to(
cyl_original_vector, cyl_destination_vector)
flat_matrix = matrix[0] + matrix[1] + matrix[2]
geom_cyl.setRotation(flat_matrix)
# The rotation is around the center - thus we ignore negative
# diff values.
geom_cyl.setPosition((abs(diff_x / 2), abs(diff_y / 2),
radius - additional_distance))
geom_cyl_transform.setGeom(geom_cyl)
# sort the geoms in order of collision probability
geom.children.extend([
geom_connect_transform, geom_cyl_transform,
geom_end_transform
])
def create(self):
self.body = ode.Body(self.world)
mass = ode.Mass()
mass.setCylinder(self.d, 2, 0.3, 2.5)
geom = ode.GeomCylinder(self.space, 0.3, 2.5)
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 + 1.25, z))
self.viz.addGeom(geom)
self.viz.GetProperty(self.body).SetColor(self.treeColor)
def createWheels(wx, wz):
global viz
wheel = ode.Body(world)
mWheel = ode.Mass()
mWheel.setCylinder(100, 3, 0.5, 1) #density, direction(1,2,3), r, h
wheel.setMass(mWheel)
geomWheel = ode.GeomCylinder(space, 0.5, 1)
geomWheel.setBody(wheel)
wheel.setPosition((wx, 0, wz))
viz.addGeom(geomWheel)
viz.GetProperty(wheel).SetColor(1, 0, 0)
return wheel, geomWheel
def create_cylinder(self,key,density,length,radius,pos,base=0,rot=0,R=0.):
"""Creates a cylinder body and corresponding geom.
Arguments:
key : number id to assign to the cylinder
density : density of the given body
length : length of the cylinder
radius : radius of the cylinder
pos : position of the center of the cylinder (x,y,z list)
base : place new object at negative end of base object
"""
# Auto label the joint key or not.
key = len(self.bodies) if key == -1 else key
# create cylinder body (aligned along the z-axis so that it matches the
# GeomCylinder created below, which is aligned along the z-axis by
# default)
body = ode.Body(self.world)
M = ode.Mass()
M.setCylinder(density, 3, radius, length)
body.setMass(M)
# create a cylinder geom for collision detection
geom = ode.GeomCylinder(self.space, radius, length)
geom.setBody(body)
# set the position of the cylinder
body.setPosition((pos[0],pos[1],pos[2]))
# set parameters for drawing the body
body.shape = "cylinder"
body.length = length
body.radius = radius
# set the rotation of the cylinder
if(rot):
body.setRotation(self.form_rotation(rot))
# set the rotation of the cylinder directly
if R:
body.setRotation(R)
self.bodies[key] = body
self.geoms[key] = geom
if(base):
Placement.place_object(self.bodies[base],body)
if(self.fluid_dynamics):
self.create_surfaces(key,1.)
return key
def create(self):
self.body = ode.Body(self.world)
mass = ode.Mass()
mass.setCylinder(self.d, 2, TREEPOLERADIUS, TREEPOLEHEIGHT)
geom = ode.GeomCylinder(self.space, TREEPOLERADIUS, TREEPOLEHEIGHT)
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 + 1.25, z))
self.viz.addGeom(geom)
#self.viz.stlGeom(self.viz.GetObject(geom), 'Images/tree.stl')
self.viz.GetProperty(self.body).SetColor(self.color)
def createWheels(self, wx, wz):
wheel = ode.Body(self.world)
mWheel = ode.Mass()
mWheel.setCylinder(PERSONDENSITY, 3, SPHERERADIUS,
SPHERERADIUS) #density, direction(1,2,3), r, h
wheel.setMass(mWheel)
geomWheel = ode.GeomCylinder(self.space, SPHERERADIUS, SPHERERADIUS)
geomWheel.setBody(wheel)
wheel.setPosition((wx, SPHERERADIUS, wz))
self.viz.addGeom(geomWheel)
self.viz.GetProperty(wheel).SetColor(WHEELCOLOR)
return wheel, geomWheel
def create(self):
self.body = ode.Body(self.world)
mass = ode.Mass()
mass.setCylinderTotal(75, 2, PERSONRADIUS, PERSONHEIGHT)
geom = ode.GeomCylinder(self.space, PERSONRADIUS, PERSONHEIGHT)
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 + PERSONHEIGHT / 2 + SPHERERADIUS, z))
self.viz.addGeom(geom)
self.viz.GetProperty(self.body).SetColor(PERSONCOLOR)
#create spheres
self.w1, gw1 = self.createSphere(x + PERSONRADIUS - SPHERERADIUS, z)
self.w2, gw2 = self.createSphere(x - PERSONRADIUS + SPHERERADIUS, z)
#s3, gs3 = self.createSphere(x, z + PERSONRADIUS - SPHERERADIUS)
self.s4, gs4 = self.createSphere(x, z - PERSONRADIUS + SPHERERADIUS)
#create joints
self.j1 = ode.Hinge2Joint(self.world)
self.j1.attach(self.w1, self.body)
self.j1.setAnchor((x + PERSONRADIUS - SPHERERADIUS, SPHERERADIUS, z))
self.j1.setAxis1(self.axis3)
self.j1.setAxis2(self.axis2)
self.j2 = ode.Hinge2Joint(self.world)
self.j2.attach(self.w2, self.body)
self.j2.setAnchor((x - PERSONRADIUS + SPHERERADIUS, SPHERERADIUS, z))
self.j2.setAxis1(self.axis3)
self.j2.setAxis2(self.axis2)
'''j3 = ode.Hinge2Joint(self.world)
j3.attach(s3, self.body)
j3.setAnchor((x, SPHERERADIUS, z + PERSONRADIUS - SPHERERADIUS))
j3.setAxis1(self.axis3)
j3.setAxis2(self.axis2)'''
j4 = ode.Hinge2Joint(self.world)
j4.attach(self.s4, self.body)
j4.setAnchor((x, SPHERERADIUS, z - PERSONRADIUS + SPHERERADIUS))
j4.setAxis1(self.axis3)
j4.setAxis2(self.axis2)
#add person and road to allGroups
allGroups.append([self.w1, self.body])
allGroups.append([self.w2, self.body])
#allGroups.append([s3, self.body])
allGroups.append([self.s4, self.body])
allGroups.append([self.w1, self.road])
allGroups.append([self.w2, self.road])
#allGroups.append([s3, self.road])
allGroups.append([self.s4, self.road])
self.setLinearVelocity()
def DefineCylinder(self, Density, Radius, Length):
"""Define this element as an ode Cylinder."""
if self._hasGeom:
self._geom = ode.GeomCylinder(None, Radius, Length)
cyl = visual.cylinder(pos=(0, 0, -Length / 2.0),
axis=(0, 0, Length),
radius=Radius)
DisplayElement.SetDisplayObject(self, cyl)
self._mass = ode.Mass()
self._mass.setCylinderTotal(Density, 3, Radius,
Length) # 3 is z-axis -- same as geom
return self
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 createWheels(self, wx, wz, r=0, l=0):
wheel = ode.Body(self.world)
mWheel = ode.Mass()
mWheel.setCylinder(100, 3, WHEELRADIUS + r,
WHEELWIDTH + l) #density, direction(1,2,3), r, h
wheel.setMass(mWheel)
geomWheel = ode.GeomCylinder(self.space, WHEELRADIUS + r,
WHEELWIDTH + l)
geomWheel.setBody(wheel)
wheel.setPosition(
(wx + self.position[0], WHEELRADIUS + self.position[1],
wz + self.position[2]))
self.viz.addGeom(geomWheel)
self.viz.GetProperty(wheel).SetColor(WHEELCOLOR)
return wheel, geomWheel
def DefineCylinder(self, ElementKey, Body, Density, Radius, Length):
"""Define this element as an ode Cylinder."""
if self._hasGeom:
self._geom = ode.GeomCylinder(_bigSpace, Radius, Length)
cyl = ivisual.cylinder(frame=Body._myFrame,
pos=(0, 0, -Length / 2.0),
axis=(0, 0, Length),
radius=Radius)
DisplayElement.SetDisplayObject(self, cyl)
self._mass = ode.Mass()
self._mass.setCylinderTotal(Density, 3, Radius,
Length) # 3 is z-axis -- same as geom
self.AddGDMElement(Body, ElementKey)
return self
def __init__(self,
sim,
density=1000,
direction=3,
radius=1.0,
length=1.0,
label=None,
**kw):
# direction: 1=x, 2=y, 3=z
body = ode.Body(sim.world)
M = ode.Mass()
M.setCylinder(density, direction, radius, length)
body.setMass(M)
geom = ode.GeomCylinder(sim.space, radius, length)
geom.setBody(body)
self.size = (radius, radius, length)
BodyItem.__init__(self, sim, body, geom, label, **kw)
def _create_cue(world,
cue_mass,
cue_radius,
cue_length,
space=None,
kinematic=True):
body = ode.Body(world)
mass = ode.Mass()
mass.setCylinderTotal(cue_mass, 3, cue_radius, cue_length)
body.setMass(mass)
body.shape = "cylinder"
if kinematic:
body.setKinematic()
if space:
geom = ode.GeomCylinder(space=space,
radius=cue_radius,
length=cue_length)
geom.setBody(body)
return body, geom
return body
def extend_shape(diff_x, diff_y, diff_z):
reset_shape()
# see http://mathworld.wolfram.com/RotationMatrix.html
hypotenuse = sqrt(diff_x * diff_x + diff_y * diff_y)
# Some paths contain two identical points (e.g. a "touch" of
# the PushCutter) We don't need any extension for these.
if hypotenuse == 0:
return
cosinus = diff_x / hypotenuse
sinus = diff_y / hypotenuse
# create the cyclinder at the other end
geom_end_transform = ode.GeomTransform(geom.space)
geom_end_transform.setBody(geom.getBody())
geom_end = ode.GeomCylinder(None, radius, height)
geom_end.setPosition((diff_x, diff_y, diff_z + center_height))
geom_end_transform.setGeom(geom_end)
# create the block that connects to two cylinders at the end
rot_matrix_box = (cosinus, sinus, 0.0, -sinus, cosinus, 0.0,
0.0, 0.0, 1.0)
geom_connect_transform = ode.GeomTransform(geom.space)
geom_connect_transform.setBody(geom.getBody())
geom_connect = ode_physics.get_parallelepiped_geom(
((-hypotenuse / 2, radius, -diff_z / 2),
(hypotenuse / 2, radius, diff_z / 2),
(hypotenuse / 2, -radius, diff_z / 2),
(-hypotenuse / 2, -radius, -diff_z / 2)),
((-hypotenuse / 2, radius,
self.height - diff_z / 2),
(hypotenuse / 2,
radius, self.height + diff_z / 2),
(hypotenuse / 2, -radius,
self.height + diff_z / 2),
(-hypotenuse / 2, -radius,
self.height - diff_z / 2)))
geom_connect.setRotation(rot_matrix_box)
geom_connect.setPosition((hypotenuse / 2, 0, radius))
geom_connect_transform.setGeom(geom_connect)
# sort the geoms in order of collision probability
geom.children.extend([geom_connect_transform,
geom_end_transform])
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with PyCAM. If not, see <http://www.gnu.org/licenses/>.
"""
from pycam.Geometry.Triangle import Triangle
from pycam.Geometry.utils import number
import uuid
try:
import ode
except ImportError:
ode = None
ShapeCylinder = lambda radius, height: ode.GeomCylinder(None, radius, height)
ShapeCapsule = lambda radius, height: \
ode.GeomCapsule(None, radius, height - (2 * radius))
_ode_override_state = None
def generate_physics(models, cutter, physics=None):
if physics is None:
physics = PhysicalWorld()
physics.reset()
if not isinstance(models, (list, set, tuple)):
models = [models]
for model in models:
physics.add_mesh(model.triangles())
shape_info = cutter.get_shape("ODE")
def create_objects(self, world):
print 'chassis'
fmax = 5000 * 1.6
scale = 0.04
# chassis
density = 50.5
lx, ly, lz = (145 * scale, 10 * scale, 177 * scale)
# Create body
body = ode.Body(world.world)
mass = ode.Mass()
mass.setBox(density, lx, ly, lz)
body.setMass(mass)
chassis_mass = lx * ly * lz * density
print "chassis mass =", chassis_mass
# Set parameters for drawing the body
body.shape = "box"
body.boxsize = (lx, ly, lz)
# Create a box geom for collision detection
geom = ode.GeomBox(world.space, lengths=body.boxsize)
geom.setBody(body)
#body.setPosition((0, 3, 0))
world.add_body(body)
world.add_geom(geom)
self._chassis_body = body
density = 4
# left wheel
radius = 25 * scale
height = radius * 0.8
wheel_mass_ = math.pi * radius**2 * height * density
print "wheel mass =", wheel_mass_
px = lx / 2
py = 0
print 'left wheels'
for pz in (-(lz / 2), 0, lz / 2):
# cylinders
left_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
wheel_mass.setCylinder(density, 1, radius, height)
left_wheel_body.setMass(wheel_mass)
left_wheel_geom = ode.GeomCylinder(world.space,
radius=radius,
length=height)
left_wheel_geom.setBody(left_wheel_body)
left_wheel_body.setRotation((0, 0, 1, 0, 1, 0, -1, 0, 0))
left_wheel_body.setPosition((px - height / 2, py, pz))
# spheres
#left_wheel_body = ode.Body(world.world)
#wheel_mass = ode.Mass()
#wheel_mass.setSphere(density, radius)
#left_wheel_body.setMass(wheel_mass)
#left_wheel_geom = ode.GeomSphere(world.space, radius=radius)
#left_wheel_geom.setBody(left_wheel_body)
#left_wheel_body.setPosition((px, py, pz))
world.add_body(left_wheel_body)
world.add_geom(left_wheel_geom)
left_wheel_joint = ode.HingeJoint(world.world)
left_wheel_joint.attach(body, left_wheel_body)
left_wheel_joint.setAnchor(left_wheel_body.getPosition())
left_wheel_joint.setAxis((-1, 0, 0))
left_wheel_joint.setParam(ode.ParamFMax, fmax)
self._left_wheel_joints.append(left_wheel_joint)
print 'right wheels'
# right wheel
px = -(lx / 2)
for pz in (-(lz / 2), 0, lz / 2):
# cylinders
right_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
wheel_mass.setCylinder(density, 1, radius, height)
right_wheel_body.setMass(wheel_mass)
right_wheel_geom = ode.GeomCylinder(world.space,
radius=radius,
length=height)
right_wheel_geom.setBody(right_wheel_body)
right_wheel_body.setRotation((0, 0, 1, 0, 1, 0, -1, 0, 0))
right_wheel_body.setPosition((px - height / 2, py, pz))
# spheres
#right_wheel_body = ode.Body(world.world)
#wheel_mass = ode.Mass()
#wheel_mass.setSphere(density, radius)
#right_wheel_body.setMass(wheel_mass)
#right_wheel_geom = ode.GeomSphere(world.space, radius=radius)
#right_wheel_geom.setBody(right_wheel_body)
#right_wheel_body.setPosition((px, py, pz))
world.add_body(right_wheel_body)
world.add_geom(right_wheel_geom)
right_wheel_joint = ode.HingeJoint(world.world)
right_wheel_joint.attach(body, right_wheel_body)
right_wheel_joint.setAnchor(right_wheel_body.getPosition())
right_wheel_joint.setAxis((-1, 0, 0))
right_wheel_joint.setParam(ode.ParamFMax, fmax)
self._right_wheel_joints.append(right_wheel_joint)
print "total mass =", float(chassis_mass + 6 * wheel_mass_)
def create_ode_geom(self, space):
return ode.GeomCylinder(space=space,
radius=self.radius,
length=self.height)
def _loadObjects(self): """ Spawns the Differential Drive robot based on the input pose. """ # Create a Box box_pos = [self.basepos[0], self.basepos[1] + self.cubesize, self.basepos[2]] rot = self.baserot boxbody = ode.Body(self.world) boxgeom = ode.GeomBox(space=self.space, lengths=(self.cubesize, self.cubesize*0.75, self.cubesize) ) boxgeom.setBody(boxbody) boxgeom.setPosition(box_pos) boxgeom.setRotation(rot) boxM = ode.Mass() boxM.setBox(self.cubemass,self.cubesize,self.cubesize*0.75,self.cubesize) boxbody.setMass(boxM) # Create two Cylindrical Wheels rot = self.multmatrix(self.genmatrix(math.pi/2,2),rot) leftwheel_pos = [box_pos[0] + 0.5*self.track, box_pos[1] - self.wheelradius*0.5, box_pos[2] + self.cubesize*0.2] rightwheel_pos = [box_pos[0] - 0.5*self.track, box_pos[1] - self.wheelradius*0.5, box_pos[2] + self.cubesize*0.2] leftwheelbody = ode.Body(self.world) leftwheelgeom = ode.GeomCylinder(space=self.space, radius = self.wheelradius, length = self.wheelradius/3.0 ) leftwheelgeom.setBody(leftwheelbody) leftwheelgeom.setPosition(leftwheel_pos) leftwheelgeom.setRotation(rot) leftwheelM = ode.Mass() leftwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius, self.wheelradius/3.0) leftwheelbody.setMass(leftwheelM) lefthinge = ode.HingeJoint(self.world) lefthinge.attach(leftwheelbody,boxbody) lefthinge.setAnchor(leftwheel_pos) lefthinge.setAxis(self.rotate((0,0,1),rot)) lefthinge.setParam(ode.ParamFMax,self.hingemaxforce) rightwheelbody = ode.Body(self.world) rightwheelgeom = ode.GeomCylinder(space=self.space, radius = self.wheelradius, length = self.wheelradius ) rightwheelgeom.setBody(rightwheelbody) rightwheelgeom.setPosition(rightwheel_pos) rightwheelgeom.setRotation(rot) rightwheelM = ode.Mass() rightwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius, self.cubemass/3.0) rightwheelbody.setMass(rightwheelM) righthinge = ode.HingeJoint(self.world) righthinge.attach(rightwheelbody,boxbody) righthinge.setAnchor(rightwheel_pos) righthinge.setAxis(self.rotate((0,0,1),rot)) righthinge.setParam(ode.ParamFMax,self.hingemaxforce) # Create a spherical wheel at the back for support caster_pos = [box_pos[0], box_pos[1] - self.cubesize*0.5, box_pos[2] - self.cubesize*0.5] casterbody = ode.Body(self.world) castergeom = ode.GeomSphere(space=self.space, radius = self.cubesize/5.0) castergeom.setBody(casterbody) castergeom.setPosition(caster_pos) castergeom.setRotation(rot) casterM = ode.Mass() casterM.setSphere(self.cubemass*100.0, self.cubesize/5.0) casterbody.setMass(casterM) self.fixed = ode.FixedJoint(self.world) self.fixed.attach(casterbody,boxbody) self.fixed.setFixed() # WHEW, THE END OF ALL THAT FINALLY! # Build the Geoms and Joints arrays for rendering. self.boxgeom = boxgeom self._geoms = [boxgeom, leftwheelgeom, rightwheelgeom, castergeom, self.ground] self.lefthinge = lefthinge self.righthinge = righthinge self._joints = [self.lefthinge, self.righthinge, self.fixed]
def __init__(self,
world,
space,
shape,
pos,
size=[],
bounciness=1,
friction=0,
vertices=None,
indices=None):
self.node3D = viz.addGroup()
# If it is NOT linked it updates seld.node3D pose with pos/quat pose on each frame
# If it is NOT linked it updates pos/quat pose with node3D pose on each frame
# This allows a linked phsynode to be moved around by an external source via the node3D
self.isLinked = 0
self.geom = 0
self.body = 0
self.parentWorld = []
self.parentSpace = []
self.bounciness = bounciness
self.friction = friction
# A list of bodies that it will stick to upon collision
self.stickTo_gIdx = []
self.collisionPosLocal_XYZ = []
if shape == 'plane':
# print 'phsEnv.createGeom(): type=plane expects pos=ABCD,and NO size. SIze is auto infinite.'
self.geom = ode.GeomPlane(space, [pos[0], pos[1], pos[2]], pos[3])
self.parentSpace = space
# No more work needed
elif shape == 'sphere':
#print 'Making sphere physNode'
# print 'phsEnv.createGeom(): type=sphere expects pos=XYZ, and size=RADIUS'
################################################################################################
################################################################################################
# Define the Body: something that moves as if under the
# influence of environmental physical forces
self.geomMass = ode.Mass()
# set sphere properties automatically assuming a mass of 1 and self.radius
mass = 1.0
self.geomMass.setSphereTotal(mass, size)
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
# Define the Geom: a geometric shape used to calculate collisions
#size = radius!
self.geom = ode.GeomSphere(space, size)
self.geom.setBody(self.body)
self.parentSpace = space
################################################################################################
################################################################################################
#elif shape == 'cylinder':
elif ('cylinder' in shape):
#print 'Making cylinder physNode'
# print 'phsEnv.createGeom(): type=sphere expects pos=XYZ, and size=RADIUS'
################################################################################################
################################################################################################
# Define the Body: something that moves as if under the
# influence of environmental physical forces
radius = size[1]
length = size[0]
self.geomMass = ode.Mass()
# set sphere properties automatically assuming a mass of 1 and self.radius
mass = 1.0
if (shape[-2:] == '_X'):
direction = 1
elif (shape[-2:] == '_Y'):
direction = 2
else:
direction = 3 # direction - The direction of the cylinder (1=x axis, 2=y axis, 3=z axis)
self.geomMass.setCylinderTotal(mass, direction, radius, length)
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
# Define the Geom: a geometric shape used to calculate collisions
#size = radius!
self.geom = ode.GeomCylinder(space, radius, length)
self.geom.setPosition(pos)
self.geom.setBody(self.body)
# This bit compensates for a problem with ODE
# Note how the body is created in line with any axis
# When I wrote this note, it was in-line with Y (direction=2)
# The geom, however, can only be made in-line with the Z axis
# This creates an offset to bring the two in-line
vizOffsetTrans = viz.Transform()
if (shape[-2:] == '_X'):
vizOffsetTrans.setAxisAngle(1, 0, 0, 90)
elif (shape[-2:] == '_Y'):
vizOffsetTrans.setAxisAngle(0, 0, 1, 90)
vizOffsetQuat = vizOffsetTrans.getQuat()
odeRotMat = self.vizQuatToRotationMat(vizOffsetQuat)
#print self.geom.getRotation()
self.geom.setOffsetRotation(odeRotMat)
self.parentSpace = space
elif shape == 'box':
################################################################################################
################################################################################################
# Define the Body: something that moves as if under the
# influence of environmental physical forces
length = size[1]
width = size[2]
height = size[0]
self.geomMass = ode.Mass()
# set sphere properties automatically assuming a mass of 1 and self.radius
mass = 1.0
self.geomMass.setBoxTotal(mass, length, width, height)
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
# Define the Geom: a geometric shape used to calculate collisions
#size = radius!
self.geom = ode.GeomBox(space, [length, width, height])
self.geom.setPosition(pos)
self.geom.setBody(self.body)
self.parentSpace = space
elif shape == 'trimesh':
if (vertices == None or indices == None):
print 'physNode.init(): For trimesh, must pass in vertices and indices'
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
triMeshData = ode.TrisMeshData()
triMeshData.build(vertices, indices)
self.geom = ode.GeomTriMesh(td, space)
self.geom.setBody(self.body)
## Set parameters for drawing the trimesh
body.shape = "trimesh"
body.geom = self.geom
self.parentSpace = space
else:
print 'physEnv.physNode.init(): ' + str(
type) + ' not implemented yet!'
return
pass
#print '**************UPDATING THE NODE *****************'
self.updateNodeAct = vizact.onupdate(viz.PRIORITY_PHYSICS,
self.updateNode3D)
def addBody(self, p1, p2, radius, type='cylinder'):
p1 = add3(p1, self.offset)
p2 = add3(p2, self.offset)
length = dist3(p1, p2)
body = ode.Body(self.world)
m = ode.Mass()
if type == 'cylinder':
m.setCylinder(self.density, 3, radius, length)
body.setMass(m)
body.shape = "cylinder"
body.length = length
body.radius = radius
geom = ode.GeomCylinder(self.space, radius, length)
geom.setBody(body)
if type == 'hbar':
m.setBox(self.density, H_BAR_LEN, radius, H_BAR_THICKNESS)
body.shape = "box"
body.length = length
body.width = radius
body.height = H_BAR_THICKNESS
body.len = H_BAR_LEN
geom = ode.GeomBox(self.space,
lengths=(H_BAR_LEN, radius, H_BAR_THICKNESS))
geom.setBody(body)
if type == 'vbar':
m.setBox(self.density, V_BAR_LEN, radius, V_BAR_THICKNESS)
body.shape = "box"
body.length = length
body.width = radius
body.height = V_BAR_THICKNESS
body.len = V_BAR_LEN
geom = ode.GeomBox(self.space,
lengths=(H_BAR_LEN, radius, H_BAR_THICKNESS))
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 __init__(self, bones: List[mmdpy_bone.mmdpyBone],
bodies: List[mmdpy_type.mmdpyTypePhysicsBody],
joints: List[mmdpy_type.mmdpyTypePhysicsJoint]):
self.bones: List[mmdpy_bone.mmdpyBone] = bones
self.bodies: List[mmdpy_type.mmdpyTypePhysicsBody] = bodies
self.joints: List[mmdpy_type.mmdpyTypePhysicsJoint] = joints
self.ode_bodies: List[Any] = []
self.ode_joints: List[Any] = []
self.ode_geoms: List[Any] = []
# 世界生成
self.world = ode.World()
self.world.setGravity([0, -9.81, 0])
self.world.setERP(0.80)
self.world.setCFM(1e-5)
# Create a space object
self.space = ode.Space()
# A joint group for the contact joints that are generated whenever
# two bodies collide
self.contactgroup = ode.JointGroup()
# 剛体
for i, body in enumerate(self.bodies):
body.cid = i
body.bone = self.bones[body.bone_id]
ode_body = ode.Body(self.world)
mass = ode.Mass()
density: float = 120.0
ode_geom = None
if body.type_id == 0: # 球
mass.setSphere(density, body.sizes[0])
ode_geom = ode.GeomSphere(self.space, radius=body.sizes[0])
if body.type_id == 1: # 箱
mass.setBox(density, body.sizes[0], body.sizes[1],
body.sizes[2])
ode_geom = ode.GeomBox(self.space, lengths=body.sizes)
if body.type_id == 2: # カプセル
mass.setCylinder(density, 2, body.sizes[0], body.sizes[1])
ode_geom = ode.GeomCylinder(self.space,
radius=body.sizes[0],
length=body.sizes[1])
mass.mass = (1 if body.calc == 0 else body.mass / density)
ode_body.setMass(mass)
if ode_geom is not None:
ode_geom.setBody(ode_body)
ode_body.setPosition(body.pos)
quat: np.ndarray = scipy.spatial.transform.Rotation.from_rotvec(
body.rot).as_matrix().reshape(9)
ode_body.setRotation(quat)
# debug
body.calc = 0
self.ode_bodies.append(ode_body)
self.ode_geoms.append(ode_geom)
# joint
# https://so-zou.jp/robot/tech/physics-engine/ode/joint/
for i, joint in enumerate(self.joints[:1]):
joint.cid = i
body_a = self.ode_bodies[joint.rigidbody_a]
body_b = self.ode_bodies[joint.rigidbody_b]
ode_joint = ode.BallJoint(self.world)
# ode_joint = ode.FixedJoint(self.world)
# ode_joint = ode.HingeJoint(self.world)
# ode_joint.attach(ode.environment, body_b)
ode_joint.attach(body_a, body_b)
ode_joint.setAnchor(joint.pos)
# ode_joint = ode.UniversalJoint(self.world)
# ode_joint.attach(body_a, body_b)
# ode_joint.setAnchor(joint.pos)
# ode_joint.setAxis1(joint.rot)
# ode_joint.setParam(ode.ParamLoStop, joint.)
self.ode_joints.append(ode_joint)
# # 接続確認
# print(
# self.bodies[joint.rigidbody_a].name,
# self.bodies[joint.rigidbody_b].name,
# ode.areConnected(body_a, body_b)
# )
# print(joint.pos, body_b.getPosition(), self.bodies[joint.rigidbody_b].pos)
# debug
self.bodies[joint.rigidbody_b].calc = 1
for i, body in enumerate(self.bodies):
ode_body = self.ode_bodies[i]
if body.calc == 0:
ode_joint = ode.BallJoint(self.world)
ode_joint.attach(ode.environment, ode_body)
ode_joint.setAnchor(body.pos)
self.ode_joints.append(ode_joint)
# 時刻保存
self.prev_time: float = time.time()
def get_shape(self, engine="ODE"):
if engine == "ODE":
import ode
import pycam.Physics.ode_physics as ode_physics
""" We don't handle the the "additional_distance" perfectly, since
the "right" shape would be a cylinder with a small flat cap that
grows to the full expanded radius through a partial sphere. The
following ascii art shows the idea:
| |
\_/
This slight incorrectness should be neglectable and causes no harm.
"""
additional_distance = self.get_required_distance()
radius = self.distance_radius
height = self.height + additional_distance
center_height = height / 2 - additional_distance
geom = ode.GeomTransform(None)
geom_drill = ode.GeomCylinder(None, radius, height)
geom_drill.setPosition((0, 0, center_height))
geom.setGeom(geom_drill)
geom.children = []
def reset_shape():
geom.children = []
def set_position(x, y, z):
geom.setPosition((x, y, z))
def extend_shape(diff_x, diff_y, diff_z):
reset_shape()
# see http://mathworld.wolfram.com/RotationMatrix.html
hypotenuse = sqrt(diff_x * diff_x + diff_y * diff_y)
# Some paths contain two identical points (e.g. a "touch" of
# the PushCutter) We don't need any extension for these.
if hypotenuse == 0:
return
cosinus = diff_x / hypotenuse
sinus = diff_y / hypotenuse
# create the cyclinder at the other end
geom_end_transform = ode.GeomTransform(geom.space)
geom_end_transform.setBody(geom.getBody())
geom_end = ode.GeomCylinder(None, radius, height)
geom_end.setPosition((diff_x, diff_y, diff_z + center_height))
geom_end_transform.setGeom(geom_end)
# create the block that connects to two cylinders at the end
rot_matrix_box = (cosinus, sinus, 0.0, -sinus, cosinus, 0.0,
0.0, 0.0, 1.0)
geom_connect_transform = ode.GeomTransform(geom.space)
geom_connect_transform.setBody(geom.getBody())
geom_connect = ode_physics.get_parallelepiped_geom(
(Point(-hypotenuse / 2, radius, -diff_z / 2),
Point(hypotenuse / 2, radius, diff_z / 2),
Point(hypotenuse / 2, -radius, diff_z / 2),
Point(-hypotenuse / 2, -radius, -diff_z / 2)),
(Point(-hypotenuse / 2, radius, self.height - diff_z / 2),
Point(hypotenuse / 2, radius, self.height + diff_z / 2),
Point(hypotenuse / 2, -radius, self.height + diff_z / 2),
Point(-hypotenuse / 2, -radius,
self.height - diff_z / 2)))
geom_connect.setRotation(rot_matrix_box)
geom_connect.setPosition((hypotenuse / 2, 0, radius))
geom_connect_transform.setGeom(geom_connect)
# sort the geoms in order of collision probability
geom.children.extend(
[geom_connect_transform, geom_end_transform])
geom.extend_shape = extend_shape
geom.reset_shape = reset_shape
self.shape[engine] = (geom, set_position)
return self.shape[engine]
def __init__(self, u, x, z):
Movable.__init__(self,
u) #inits ode, inventor etc and register with world
RawGLObject.__init__(
self, u
) #eg may want to overlay some HC output as GL graphics onto scene to show what I'm thinking about
mass = ode.Mass()
density = 100
axis = 0 #1x;2y;3z. But seems to be ignored? (maybe as physObject sets rotation elsewhere?)
radius = 0.5
depth = 0.1
mass.setCylinder(density, axis, radius, depth)
self.body = ode.Body(self.u.world)
self.body.setMass(mass)
self.body.setPosition((x, 0.3, z))
self.geom = ode.GeomCylinder(u.robotSpace, radius, depth)
self.geom.setBody(self.body)
th = math.pi / 2
a = math.sin(th / 2)
self.geom.setQuaternion((math.cos(th / 2), a * 1, a * 0, a * 0))
# self.sg.addChild(makeRobotSceneGraph())
# self.sg.addChild(makeRatSceneGraph())
#NB: ODE cylinders are in z-axis by default; Inventor are in y!
#It MUST be Inventor that is modified, as ODE rotations are drawn anyway by Inventor
rot = SoRotation()
th = SbRotation(SbVec3f(1, 0, 0), 3.142 / 2)
rot.rotation = th
self.sg.addChild(rot)
myCylinder = SoCylinder() #TODO rotate zo z axis height is default
myCylinder.radius = radius
myCylinder.height = depth
self.sg.addChild(myCylinder)
#simple 'head' to show the front
trans = SoTransform()
trans.translation.setValue(0.5, -0.2, 0.0)
trans.scaleFactor.setValue(.2, .2, .2)
self.sg.addChild(trans)
self.sg.addChild(SoSphere())
# self.controller = SimpleController(self)
self.controller = IceaController(self)
# self.controller = BrahmsController(self)
self.F_controller = (
0.0, 0.0, 0.0
) #Force due to controller. cache these so we dont have to run controller on every physics step
self.T_controller = (0.0, 0.0, 0.0) #Torque due to controller.
self.controller_bSetoff = False
u.robots.append(self)
self.sensors = [] #append sensors to this list!
self.segs = []
self.follicleL = Segment(u, self, 1)
self.follicleR = Segment(u, self, -1)
self.segs.append(self.follicleL)
self.segs.append(self.follicleR)
par = self.follicleL
for i in range(2, 4):
seg = Segment(u, par, i + 1)
self.segs.append(seg)
par = seg
par = self.follicleR
for i in range(2, 4):
seg = Segment(u, par, -i)
self.segs.append(seg)
par = seg
self.wheelL = Wheel(u, self, -0.6)
self.wheelR = Wheel(u, self, 0.6)
M.setBox(100, 1, 2, 3)
#M.mass = 1.0
Tree = ode.Mass()
Tree.setCylinder(100, 3, 3, 10)
Treebody = ode.Body(world)
Treebody.setMass(Tree)
body.setMass(M)
#body.addForce((0,10,0))
body.setPosition((0, 0, 0))
#body.addForce( (0,200,0) )
geomTree = ode.GeomCylinder(space, 3, 10)
geomTree.setBody(Treebody)
geom = ode.GeomBox(space, lengths=(1, 2, 3))
geom.setBody(body)
ang = pi / 2
rotation = np.array([[1, 0, 0], [0, cos(ang), -sin(ang)],
[0, sin(ang), cos(ang)]])
print rotation.reshape(1, 9)[0]
geomTree.setRotation((rotation.reshape(1, 9)[0]))
body.setPosition((0, 1, 0))
body.setLinearVel((0.1, 0, 0))
Treebody.setPosition((10, 5, 30))
fps = 10
dt = 0.1
def create_objects(self, world):
print 'chassis'
# chassis
density = 10
lx, ly, lz = (8, 0.5, 8)
# Create body
body = ode.Body(world.world)
mass = ode.Mass()
mass.setBox(density, lx, ly, lz)
body.setMass(mass)
# Set parameters for drawing the body
body.shape = "box"
body.boxsize = (lx, ly, lz)
# Create a box geom for collision detection
geom = ode.GeomBox(world.space, lengths=body.boxsize)
geom.setBody(body)
#body.setPosition((0, 3, 0))
world.add_body(body)
world.add_geom(geom)
self._chassis_body = body
density = 1
print 'left wheel'
# left wheel
radius = 1
height = 0.2
px, py, pz = (lx / 2, 0, -(lz / 2))
left_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
#wheel_mass.setSphere(density, radius)
wheel_mass.setCylinder(density, 1, radius, height)
left_wheel_body.setMass(wheel_mass)
#left_wheel_geom = ode.GeomSphere(world.space, radius=radius)
left_wheel_geom = ode.GeomCylinder(world.space, radius=radius,
length=height)
left_wheel_geom.setBody(left_wheel_body)
#left_wheel_body.setPosition((px, py, pz))
left_wheel_body.setRotation((0, 0, 1,
0, 1, 0,
-1, 0, 0))
left_wheel_body.setPosition((px - height / 2, py, pz))
world.add_body(left_wheel_body)
world.add_geom(left_wheel_geom)
print 'right wheel'
# right wheel
#radius = 1
px = -lx / 2
right_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
#wheel_mass.setSphere(density, radius)
wheel_mass.setCylinder(density, 1, radius, height)
right_wheel_body.setMass(wheel_mass)
#right_wheel_geom = ode.GeomSphere(world.space, radius=radius)
right_wheel_geom = ode.GeomCylinder(world.space, radius=radius,
length=height)
right_wheel_geom.setBody(right_wheel_body)
#right_wheel_body.setPosition((px, py, pz))
right_wheel_body.setRotation((0, 0, 1,
0, 1, 0,
-1, 0, 0))
right_wheel_body.setPosition((px - height / 2, py, pz))
world.add_body(right_wheel_body)
world.add_geom(right_wheel_geom)
print 'front wheel'
# front wheel
#radius = 1
px, py, pz = (0, 0, lz / 2)
front_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
wheel_mass.setSphere(density, radius)
front_wheel_body.setMass(wheel_mass)
front_wheel_geom = ode.GeomSphere(world.space, radius=radius)
front_wheel_geom.setBody(front_wheel_body)
front_wheel_body.setPosition((px, py, pz))
world.add_body(front_wheel_body)
world.add_geom(front_wheel_geom)
#left_wheel_joint = ode.Hinge2Joint(world.world)
left_wheel_joint = ode.HingeJoint(world.world)
left_wheel_joint.attach(body, left_wheel_body)
left_wheel_joint.setAnchor(left_wheel_body.getPosition())
left_wheel_joint.setAxis((-1, 0, 0))
#left_wheel_joint.setAxis1((0, 1, 0))
#left_wheel_joint.setAxis2((1, 0, 0))
left_wheel_joint.setParam(ode.ParamFMax, 500000)
#left_wheel_joint.setParam(ode.ParamLoStop, 0)
#left_wheel_joint.setParam(ode.ParamHiStop, 0)
#left_wheel_joint.setParam(ode.ParamFMax2, 0.1)
#left_wheel_joint.setParam(ode.ParamSuspensionERP, 0.2)
#left_wheel_joint.setParam(ode.ParamSuspensionCFM, 0.1)
self._left_wheel_joints.append(left_wheel_joint)
#right_wheel_joint = ode.Hinge2Joint(world.world)
right_wheel_joint = ode.HingeJoint(world.world)
right_wheel_joint.attach(body, right_wheel_body)
right_wheel_joint.setAnchor(right_wheel_body.getPosition())
right_wheel_joint.setAxis((-1, 0, 0))
#right_wheel_joint.setAxis1((0, 1, 0))
#right_wheel_joint.setAxis2((1, 0, 0))
right_wheel_joint.setParam(ode.ParamFMax, 500000)
#right_wheel_joint.setParam(ode.ParamLoStop, 0)
#right_wheel_joint.setParam(ode.ParamHiStop, 0)
#right_wheel_joint.setParam(ode.ParamFMax2, 0.1)
#right_wheel_joint.setParam(ode.ParamSuspensionERP, 0.2)
#right_wheel_joint.setParam(ode.ParamSuspensionCFM, 0.1)
self._right_wheel_joints.append(right_wheel_joint)
front_wheel_joint = ode.BallJoint(world.world)
front_wheel_joint.attach(body, front_wheel_body)
front_wheel_joint.setAnchor(front_wheel_body.getPosition())
front_wheel_joint.setParam(ode.ParamFMax, 5000)
def _loadObjects(self):
"""
Spawns the Differential Drive robot based on the input pose.
"""
# Create a Box
box_pos = [
self.basepos[0], self.basepos[1] + self.cubesize, self.basepos[2]
]
rot = self.baserot
boxbody = ode.Body(self.world)
boxgeom = ode.GeomBox(space=self.space,
lengths=(self.cubesize, self.cubesize * 0.75,
self.cubesize))
boxgeom.setBody(boxbody)
boxgeom.setPosition(box_pos)
boxgeom.setRotation(rot)
boxM = ode.Mass()
boxM.setBox(self.cubemass, self.cubesize, self.cubesize * 0.75,
self.cubesize)
boxbody.setMass(boxM)
# Create two Cylindrical Wheels
rot = self.multmatrix(self.genmatrix(math.pi / 2, 2), rot)
leftwheel_pos = [
box_pos[0] + 0.5 * self.track, box_pos[1] - self.wheelradius * 0.5,
box_pos[2] + self.cubesize * 0.2
]
rightwheel_pos = [
box_pos[0] - 0.5 * self.track, box_pos[1] - self.wheelradius * 0.5,
box_pos[2] + self.cubesize * 0.2
]
leftwheelbody = ode.Body(self.world)
leftwheelgeom = ode.GeomCylinder(space=self.space,
radius=self.wheelradius,
length=self.wheelradius / 3.0)
leftwheelgeom.setBody(leftwheelbody)
leftwheelgeom.setPosition(leftwheel_pos)
leftwheelgeom.setRotation(rot)
leftwheelM = ode.Mass()
leftwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius,
self.wheelradius / 3.0)
leftwheelbody.setMass(leftwheelM)
lefthinge = ode.HingeJoint(self.world)
lefthinge.attach(leftwheelbody, boxbody)
lefthinge.setAnchor(leftwheel_pos)
lefthinge.setAxis(self.rotate((0, 0, 1), rot))
lefthinge.setParam(ode.ParamFMax, self.hingemaxforce)
rightwheelbody = ode.Body(self.world)
rightwheelgeom = ode.GeomCylinder(space=self.space,
radius=self.wheelradius,
length=self.wheelradius)
rightwheelgeom.setBody(rightwheelbody)
rightwheelgeom.setPosition(rightwheel_pos)
rightwheelgeom.setRotation(rot)
rightwheelM = ode.Mass()
rightwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius,
self.cubemass / 3.0)
rightwheelbody.setMass(rightwheelM)
righthinge = ode.HingeJoint(self.world)
righthinge.attach(rightwheelbody, boxbody)
righthinge.setAnchor(rightwheel_pos)
righthinge.setAxis(self.rotate((0, 0, 1), rot))
righthinge.setParam(ode.ParamFMax, self.hingemaxforce)
# Create a spherical wheel at the back for support
caster_pos = [
box_pos[0], box_pos[1] - self.cubesize * 0.5,
box_pos[2] - self.cubesize * 0.5
]
casterbody = ode.Body(self.world)
castergeom = ode.GeomSphere(space=self.space,
radius=self.cubesize / 5.0)
castergeom.setBody(casterbody)
castergeom.setPosition(caster_pos)
castergeom.setRotation(rot)
casterM = ode.Mass()
casterM.setSphere(self.cubemass * 100.0, self.cubesize / 5.0)
casterbody.setMass(casterM)
self.fixed = ode.FixedJoint(self.world)
self.fixed.attach(casterbody, boxbody)
self.fixed.setFixed()
# WHEW, THE END OF ALL THAT FINALLY!
# Build the Geoms and Joints arrays for rendering.
self.boxgeom = boxgeom
self._geoms = [
boxgeom, leftwheelgeom, rightwheelgeom, castergeom, self.ground
]
self.lefthinge = lefthinge
self.righthinge = righthinge
self._joints = [self.lefthinge, self.righthinge, self.fixed]
def loadObstacles(self, obstaclefile):
"""
Loads obstacles from the obstacle text file.
"""
# Initiate data structures.
data = open(obstaclefile, "r")
obs_sizes = []
obs_positions = []
obs_masses = []
reading = "None"
# Parse the obstacle text file.
for line in data:
linesplit = line.split()
if linesplit != []:
if linesplit[0] != "#":
obs_sizes.append([
float(linesplit[0]),
float(linesplit[1]),
float(linesplit[2])
])
obs_positions.append([
float(linesplit[3]),
float(linesplit[4]),
float(linesplit[5])
])
obs_masses.append(float(linesplit[6]))
# Go through all the obstacles in the list and spawn them.
for i in range(len(obs_sizes)):
obs_size = obs_sizes[i]
obs_pos = obs_positions[i]
obs_mass = obs_masses[i]
# Create the obstacle.
body = ode.Body(self.world)
geom = ode.GeomBox(space=self.space, lengths=obs_size)
geom.setBody(body)
geom.setPosition(obs_pos)
M = ode.Mass()
M.setBox(obs_mass, obs_size[0], obs_size[1], obs_size[2])
body.setMass(M)
# Append all these new pointers to the simulator class.
self._geoms.append(geom)
