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)