Commits

German Larrain committed 5469d30

-simulation package "add_X" methods now create instances of physics package classes instead of calling functions of the latter
-SimulatedObject's _body member is now a simulation.Body instead of an ode.Body
-SimulatedObject's _geom member was removed because it was not needed
-collision.Geom class was removed because geometry.Shape is used
-physics package classes Body, Capsule, Cylinder, Box, Sphere and Cone were implemented
-physics package functions create_X_body_and_geom renamed to _create_ode_X, and their implementation no longer deals with geoms, because that is managed by geometry.Shape subclasses

Comments (0)

Files changed (7)

ars/model/contrib/ragdoll.py

 
 # Created on 2011.10.31
-# Last modified on 2011.11.01
 #
 # @author: german
 
 		# cylinder length not including endcaps, make capsules overlap by half radius at joints
 		cyllen = mut.dist3(p1, p2) - radius
 
-		body, geom = phs.create_capsule_body_and_geom(self.world, self.space, self.density, cyllen, radius)
+		#body, geom = phs.create_capsule_body_and_geom(self.world, self.space, self.density, cyllen, radius)
+		body = phs.Capsule(self.world, self.space, self.density, cyllen, radius)
 		
 		# define body rotation automatically from body axis
 		za = mut.norm3(mut.sub3(p2, p1))
 		ya = mut.cross_product(za, xa)
 		rot = (xa[0], ya[0], za[0], xa[1], ya[1], za[1], xa[2], ya[2], za[2])
 
-		body.setPosition(mut.mult_by_scalar3(mut.add3(p1, p2), 0.5)) # equivalent to the midpoint
-		body.setRotation(rot)
+		body.set_position(mut.mult_by_scalar3(mut.add3(p1, p2), 0.5)) # equivalent to the midpoint
+		body.set_rotation(rot)
 
 		self.bodies.append(body)
-		self.geoms.append(geom)
-		self.totalMass += body.getMass().mass
+		#self.geoms.append(geom)
+		self.totalMass += body.get_mass()
 
 		return body
 
 	def add_fixed_joint(self, body1, body2):
 		joint = ode.FixedJoint(self.world)
-		joint.attach(body1, body2)
+		joint.attach(body1.get_inner_object(), body2.get_inner_object())
 		joint.setFixed()
 
 		joint.style = "fixed"
 		anchor = mut.add3(anchor, self.offset)
 
 		joint = ode.HingeJoint(self.world)
-		joint.attach(body1, body2)
+		joint.attach(body1.get_inner_object(), body2.get_inner_object())
 		joint.setAnchor(anchor)
 		joint.setAxis(axis)
 		joint.setParam(ode.ParamLoStop, loStop)
 		anchor = mut.add3(anchor, self.offset)
 
 		joint = ode.UniversalJoint(self.world)
-		joint.attach(body1, body2)
+		joint.attach(body1.get_inner_object(), body2.get_inner_object())
 		joint.setAnchor(anchor)
 		joint.setAxis1(axis1)
 		joint.setAxis2(axis2)
 
 		# create the joint
 		joint = ode.BallJoint(self.world)
-		joint.attach(body1, body2)
+		joint.attach(body1.get_inner_object(), body2.get_inner_object())
 		joint.setAnchor(anchor)
 
 		'''

ars/model/geometry/shapes.py

 
 # Created on 2011.10.31
-# Last modified on 2011.10.31
 #
 # @author: german
 
 	'''
 	__metaclass__ = ABCMeta
 	
+	@abstractmethod
 	def __init__(self):
 		self._inner_object = None
 		self._attached_body = None 
 	'''
 	Plane, different from a box.
 	'''
-	def __init__(self):
-		self._inner_object = ode.GeomPlane()
+	def __init__(self, space, normal, dist):
+		self._inner_object = ode.GeomPlane(space.get_inner_object(), normal, dist)
 
 class Cone(Primitive):
 	'''

ars/model/robot/joints.py

 
 # Created on 2011.10.31
-# Last modified on 2011.10.31
 #
 # @author: german
 
 	'''
 	__metaclass__ = ABCMeta
 	
-#	@abstractmethod
+	@abstractmethod
 	def __init__(self, world, inner_joint, body1=None, body2=None):
 		self._world = world
 		self._inner_joint = inner_joint
 	def __init__(self, world, body1, body2):
 		try:
 			inner_joint = ode.FixedJoint(world)
-			inner_joint.attach(body1, body2)
+			inner_joint.attach(body1.get_inner_object(), body2.get_inner_object())
 			inner_joint.setFixed()
 			
 			super(Fixed, self).__init__(world, inner_joint, body1, body2)
 
 		try:
 			inner_joint = ode.HingeJoint(world)
-			inner_joint.attach(body1, body2)
+			inner_joint.attach(body1.get_inner_object(), body2.get_inner_object())
 			inner_joint.setAnchor(anchor)
 			inner_joint.setAxis(axis) # see contrib.Ragdoll.addHingeJoint for possible modification
 			
 
 		try:
 			inner_joint = ode.UniversalJoint(world)
-			inner_joint.attach(body1, body2)
+			inner_joint.attach(body1.get_inner_object(), body2.get_inner_object())
 			inner_joint.setAnchor(anchor)
 			inner_joint.setAxis1(axis1) # see contrib.Ragdoll.addHingeJoint for possible modification
 			inner_joint.setAxis2(axis2)
 
 		try:
 			inner_joint = ode.BallJoint(world)
-			inner_joint.attach(body1, body2)
+			inner_joint.attach(body1.get_inner_object(), body2.get_inner_object())
 			inner_joint.setAnchor(anchor)
 			
 			super(BallSocket, self).__init__(world, inner_joint, body1, body2)

ars/model/simulator/__init__.py

 
 # Created on 2011.10.17
-# Last modified on 2011.12.04
 #
 # @author: german
 # TODO: attribute the code sections that were copied from somewhere else
 		self.perform_sim_steps_per_frame()		
 		if self._debug:	
 			print(self.sim_time)
-			print(self._ragdoll.head.getPosition())
+			print(self._ragdoll.head.get_position())
 		#self.updateTime()
 		self.update_actors()
 
 		gFloor = gp.Box(box_size, box_center) #TODO: use normal parameter for orientation
 		gFloor.set_color(color)
 		name = "floor"
-		return self.add_object(SimulatedObject(name, actor=gFloor, geom=self._floor_geom))
+#		return self.add_object(SimulatedObject(name, actor=gFloor, geom=self._floor_geom))
+		return self.add_object(SimulatedObject(name, actor=gFloor))
 
 	def add_obstacle(self, length = 0.25, radius = 0.15): # length = 0.05, radius = 0.15
 		# create an obstacle
-		obstacle, obs_geom = phs.create_capsule_body_and_geom(self._world, self._space, density=1000, length=length, radius=radius)
+		# TODO: create this using "add_capsule"
+#		obstacle, obs_geom = phs.create_capsule_body_and_geom(self._world, self._space, density=1000, length=length, radius=radius)
+		obstacle = phs.Capsule(self._world, self._space, density=1000, length=length, radius=radius)
 		pos = (random.uniform(-0.3, 0.3), 0.2, random.uniform(-0.15, 0.2))
 		#pos = (-0.27752817947085257, 0.20000000000000001, -0.071494489985067272)
 		
 		# TODO: apply transformation using SimulatedObject methods
-		obstacle.setPosition(pos) #TODO: necessary?
-		obstacle.setRotation(mu.rightRot) #TODO: necessary?
+		obstacle.set_position(pos) #TODO: necessary?
+		obstacle.set_rotation(mu.rightRot) #TODO: necessary?
 		#print "obstacle created at %s" % (str(pos))
 
 		#g_capsule = gp.Box((2*radius, length + 2*radius, 2*radius), pos)
 		g_capsule = gp.Capsule(length, radius, pos)
 		name = "obstacle"
-		return self.add_object(SimulatedObject(name, obstacle, g_capsule, obs_geom)) #TODO: rename to actor
+		return self.add_object(SimulatedObject(name, obstacle, g_capsule)) #TODO: rename to actor
 
 	def add_ragdoll(self, density, offset):
 		self._ragdoll = cb.RagDoll(self._world, self._space, density, offset)
 		
 		for rBody in self._ragdoll.bodies:
 			#rBodySize = (rBody.radius * 2, rBody.radius * 2, rBody.length + rBody.radius * 2)
-			actor = gp.Capsule(rBody.length, rBody.radius, rBody.getPosition())
+			actor = gp.Capsule(rBody.get_length(), rBody.get_radius(), rBody.get_position())
 			name = str(rBody)
 			self.add_object(SimulatedObject(name, rBody, actor))
 
 	def add_sphere(self, density, radius, center):
-		body, geom = phs.create_sphere_body_and_geom(self._world, self._space, density, radius)
-		body.setPosition(center)
+		#body, geom = phs.create_sphere_body_and_geom(self._world, self._space, density, radius)
+		body = phs.Sphere(self._world, self._space, density, radius)
+		body.set_position(center)
 		
 		g_sphere = gp.Sphere(radius, center)
 		name = "sphere"
-		return self.add_object(SimulatedObject(name, body, g_sphere, geom))
+		return self.add_object(SimulatedObject(name, body, g_sphere))
 
 	def add_box(self, density, size, center):
-		body, geom = phs.create_box_body_and_geom(self._world, self._space, density, size)
-		body.setPosition(center)
+		#body, geom = phs.create_box_body_and_geom(self._world, self._space, density, size)
+		body = phs.Box(self._world, self._space, density, size)
+		body.set_position(center)
 		
 		g_box = gp.Box(size, center)
 		name = "box" + str(center) #FIXME
-		return self.add_object(SimulatedObject(name, body, g_box, geom))
+		return self.add_object(SimulatedObject(name, body, g_box))
 
 	def add_cone(self, density, height, radius, center):
 		'''
 		raise Exception("Not available in ODE")
 
 	def add_cylinder(self, density, length, radius, center):
-		body, geom = phs.create_cylinder_body_and_geom(self._world, self._space, density, length, radius)		
-		body.setPosition(center)
+		#body, geom = phs.create_cylinder_body_and_geom(self._world, self._space, density, length, radius)		
+		body = phs.Cylinder(self._world, self._space, density, length, radius)
+		body.set_position(center)
 
 		g_cylinder = gp.Cylinder(length, radius, center)
 		name = "cylinder"
-		return self.add_object(SimulatedObject(name, body, g_cylinder, geom))
+		return self.add_object(SimulatedObject(name, body, g_cylinder))
 
 	def add_capsule(self, density, length, radius, center):
-		body, geom = phs.create_capsule_body_and_geom(self._world, self._space, density, length, radius)		
-		body.setPosition(center)
+		#body, geom = phs.create_capsule_body_and_geom(self._world, self._space, density, length, radius)
+		body = phs.Capsule(self._world, self._space, density, length, radius)
+		body.set_position(center)
 
 		g_capsule = gp.Capsule(length, radius, center)
 		name = "capsule"
-		return self.add_object(SimulatedObject(name, body, g_capsule, geom))
+		return self.add_object(SimulatedObject(name, body, g_capsule))
 	
 	def get_actors(self):
 		""" Returns a dictionary with each object actor indexed by its name. """
 		self._name = name
 		self._body = body
 		self._actor = actor
-		self._geom = geom
 	
 	def rotate(self, axis, angle):
 		'''
 		self.set_rotation(rot_final)
 	
 	def offset_by_object(self, object_):
-		offset_pos = object_.get_body().getPosition()
+		offset_pos = object_.get_position()
 		self.offset_by_position(offset_pos)
 	
 	def offset_by_position(self, offset_pos):
-		pos = self._body.getPosition()
+		pos = self._body.get_position()
 		new_pos = mu.add3(offset_pos, pos)
 		self.set_position(new_pos)
 	
 #===============================================================================
 	
 	def set_position(self, position):
-		self._body.setPosition(position)
+		self._body.set_position(position)
 			
 	def set_rotation(self, rot_matrix):
-		self._body.setRotation(rot_matrix)
+		self._body.set_rotation(rot_matrix)
 	
 	def get_name(self):
 		return self._name
 	def get_actor(self):
 		return self._actor
 	
-	def get_geom(self):
-		return self._geom
-	
 	def get_position(self):
-		return self._body.getPosition()
+		return self._body.get_position()
 		
 	def get_rotation(self):
-		return self._body.getRotation()
+		return self._body.get_rotation()
 	
 class SimulatedJoint:
 	

ars/model/simulator/collision.py

 	def collide(self, arg, callback):
 		self._inner_object.collide(arg, callback)
 
-class Geom:
-	def __init__(self):
-		self._inner_object = None
-
 def near_callback(args, geom1, geom2):
 	"""
 	Callback function for the collide() method (in ODE). This function checks if the given geoms

ars/model/simulator/physics.py

 from abc import ABCMeta, abstractmethod
 
 import ode
-from ars.utilities import mathematical as mut
+
+#from ars.utilities import mathematical as mut
+from ars.model.geometry import shapes as shapes
 
 class Engine:
 	pass
 
 class Body:
 	__metaclass__ = ABCMeta
+	
+	@abstractmethod
+	def __init__(self):
+		self._inner_object = None
+		self._attached_geom = None
+	
+	def attach_geom(self, geom):
+		geom.attach_body(self)
+		self._attached_geom = geom
+
+#===============================================================================
+# GETTERS AND SETTERS
+#===============================================================================
+		
+	def get_inner_object(self):
+		return self._inner_object
+		
+	def get_attached_geom(self):
+		return self._attached_geom
+	
+	def get_position(self):
+		return self._inner_object.getPosition()
+	
+	def get_rotation(self):
+		return self._inner_object.getRotation()
+	
+	def get_mass(self):
+		return self._inner_object.getMass().mass
+	
+	def set_position(self, pos):
+		self._inner_object.setPosition(pos)
+	
+	def set_rotation(self, rot):
+		self._inner_object.setRotation(rot)
 
 class Capsule(Body):
-	pass
+	def __init__(self, world, space, density, length, radius):
+		'''
+		create capsule body (aligned along the z-axis so that it matches the Geom created below,
+		which is aligned along the Z-axis by default)
+		'''
+	
+		# create a capsule geom for collision detection
+		#geom = ode.GeomCCylinder(space.get_inner_object(), radius, length)
+		#geom.setBody(body)
+		
+		self._length = length
+		self._radius = radius
+		
+		body = _create_ode_capsule(world, space, density, length, radius)
+		geom = shapes.Capsule(space, length, radius)
+		self._inner_object = body
+		self.attach_geom(geom)
+	
+	def get_length(self):
+		return self._length
+	
+	def get_radius(self):
+		return self._radius
 
 class Cylinder(Body):
-	pass
+
+	def __init__(self, world, space, density, length, radius):
+		'''
+		create cylinder body (aligned along the z-axis so that it matches the Geom created below,
+		which is aligned along the Z-axis by default)
+		'''
+	
+		#geom = ode.GeomCylinder(space.get_inner_object(), radius, length)
+		#geom.setBody(body)
+		
+		self._length = length
+		self._radius = radius
+	
+		body = _create_ode_cylinder(world, space, density, length, radius)
+		geom = shapes.Cylinder(space, length, radius)
+		self._inner_object = body
+		self.attach_geom(geom)
+	
+	def get_length(self):
+		return self._length
+	
+	def get_radius(self):
+		return self._radius
 
 class Box(Body):
-	pass
+	def __init__(self, world, space, density, size):
+		self._size = size
+		
+		body = _create_ode_box(world, space, density, size)
+		geom = shapes.Box(space, size)
+		self._inner_object = body
+		self.attach_geom(geom)
+	
+	def get_size(self):
+		return self._size
 
 class Sphere(Body):
-	pass
+	def __init__(self, world, space, density, radius):
+		self._radius = radius
+		
+		body = _create_ode_sphere(world, space, density, radius)
+		geom = shapes.Sphere(space, radius)
+		self._inner_object = body
+		self.attach_geom(geom)
+	
+	def get_radius(self):
+		return self._radius
 
 class Cone(Body):
-	pass
+	def __init__(self, world, space, density, height, radius):
+		raise Exception("Not available in ODE")
 
 def create_world(gravity = (0.0, -9.81, 0.0), ERP = 0.2, CFM = 1E-10):
 	""" Create an ODE world object. """
 	world.setCFM(CFM)
 	return world
 
-def create_capsule_body_and_geom(world, space, density, length, radius):
-	'''
-	create capsule body (aligned along the z-axis so that it matches the Geom created below,
-	which is aligned along the Z-axis by default)
-	'''
+def _create_ode_capsule(world, space, density, length, radius):
+
 	capsule_direction = 3 # z-axis
 	body = ode.Body(world)
 	m = ode.Mass()
 	body.shape = "capsule" # TODO: delete this, because it is related to the original implementation
 	body.length = length
 	body.radius = radius
+	
+	return body
 
-	# create a capsule geom for collision detection
-	geom = ode.GeomCCylinder(space.get_inner_object(), radius, length)
-	geom.setBody(body)
-
-	return body, geom
-
-def create_cylinder_body_and_geom(world, space, density, length, radius):
-	'''
-	create cylinder body (aligned along the z-axis so that it matches the Geom created below,
-	which is aligned along the Z-axis by default)
-	'''
+def _create_ode_cylinder(world, space, density, length, radius):
+	
 	cylinderDirection = 3 # Z-axis
 	body = ode.Body(world)
 	m = ode.Mass()
 	m.setCylinder(density, cylinderDirection, radius, length)
 	body.setMass(m)
+	
+	return body
 
-	geom = ode.GeomCylinder(space.get_inner_object(), radius, length)
-	geom.setBody(body)
-
-	return body, geom
-
-def create_box_body_and_geom(world, space, density, size):
+def _create_ode_box(world, space, density, size):
 	body = ode.Body(world)
 	m = ode.Mass()
 	m.setBox(density, *size)
 	body.setMass(m)
 	
-	geom = ode.GeomBox(space.get_inner_object(), lengths=size)
-	geom.setBody(body)
-	return body, geom
+	return body
 
-def create_sphere_body_and_geom(world, space, density, radius):
+def _create_ode_sphere(world, space, density, radius):
 	body = ode.Body(world)
 	m = ode.Mass()
 	m.setSphere(density, radius)
 	body.setMass(m)
-	
-	geom = ode.GeomSphere(space.get_inner_object(), radius)
-	geom.setBody(body)
-	return body, geom
-
-def create_cone_body_and_geom(world, space, density, height, radius):
-	
-	raise Exception("Not available in ODE")
-	return None
+	return body
 
 def create_joint_group():
 	return ode.JointGroup()

ars/utilities/mathematical.py

 
 # Created on 2011.08.09
-# Last modified on 2011.10.25
 #
 # @author: german
 #
 	Returns the 3-vector vector transformed into the local coordinate system of ODE
 	body body.
 	"""
-	return rotate3(transpose3(body.getRotation()), vector)
+	return rotate3(transpose3(body.get_rotation()), vector)
 
 
 '''