1. German Larrain
  2. ars

Commits

German Larrain  committed 8d636d6

Partial merge with dev-sensors. Received new stable features (and modified others) developed in the 'dev-sensors' branch, such as:
-model: content of geometry was moved (and split) to collision's base and adapters modules. New 'Ray' class was created (both in base and adapters). Some functionality was added to base classes.
-model.collision.adapters.OdeEngine: substantial changes in 'near_callback' to deal with the 'Ray' special case; new static methods 'are_geoms_connected', 'is_ray' and 'handle_ray_collision'.
-model.collision.base: RayContact renamed as RayContactData and its attributes are now public.
-model.simulator.Simulation: new 'get_collision_space' method
-TODO.txt has a new task

  • Participants
  • Parent commits 3ad93b9
  • Branches dev

Comments (0)

Files changed (7)

File ars/TODO.txt

View file
  • Ignore whitespace
 TODO
 
 Code structure (modules and packages)
--
+-rename Simulation.get_joint as get_sim_joint
 
 General
 -dBodySetFiniteRotationMode

File ars/model/collision/adapters.py

View file
  • Ignore whitespace
 #
 # @author: german
 
+from abc import ABCMeta, abstractmethod
+
 import ode
 
-#from base import Engine, Space, NearCallbackArgs, RayContact
+import ars.exceptions as exc
+
 import base
 
 
 
 #	def __init__(self):
 #		pass
-	
+
 	#===========================================================================
 	# Functions and methods not overriding base class functions and methods
 	#===========================================================================
 		"""Callback function for the collide() method (in ODE). This function
 		checks if the given geoms do collide and creates contact joints (c_joint)
 		if they do, except if they are connected."""
-		
+
 		c_joint_bounce = 0.2 # default: 0.2
 		c_joint_mu = 500 # default: 500. 0-5 = very slippery, 50-500 = normal, 5000 = very sticky
-	
-		if args.ignore_connected:
-			if ode.areConnected(geom1.getBody(), geom2.getBody()):
-				return
-	
-		#===========================================================================
-		# create contact joints
-		#===========================================================================
-		#world, contact_group = args
+
 		world = args.world
 		contact_group = args.contact_group
-	
+		ray_geom = None
+		other_geom = None
+
+		if args.ignore_connected and OdeEngine.are_geoms_connected(geom1,
+			geom2):
+			return
+
+		#=======================================================================
+		# Ray's special case
+		#=======================================================================
+		if OdeEngine.is_ray(geom1):
+			if OdeEngine.is_ray(geom2):
+				print('Weird, ODE says two rays may collide. '
+					  'That case is not handled.')
+				return
+			else:
+				ray_geom = geom1
+				other_geom = geom2
+
+		elif OdeEngine.is_ray(geom2):
+			ray_geom = geom2
+			other_geom = geom1
+
+		#=======================================================================
+		# create contact joints
+		#=======================================================================
 		# check if the objects collide
 		contacts = ode.collide(geom1, geom2)
 		for c in contacts:
-			c.setBounce(c_joint_bounce)
-			c.setMu(c_joint_mu)
-			j = ode.ContactJoint(world, contact_group, c)
-			j.attach(geom1.getBody(), geom2.getBody())
+
+			if ray_geom is not None:
+				OdeEngine.handle_ray_collision(ray_geom, other_geom, c)
+			else: # we create a ContactJoint only if both geoms are not rays
+				# set contact parameters
+				c.setBounce(c_joint_bounce)
+				c.setMu(c_joint_mu)
+				# create contact joints
+				j = ode.ContactJoint(world, contact_group, c)
+				j.attach(geom1.getBody(), geom2.getBody())
+
+	@staticmethod
+	def are_geoms_connected(geom1, geom2):
+		"""Are `geom1` and `geom2` connected (through the bodies they are
+		attached to)?
+		"""
+		return ode.areConnected(geom1.getBody(), geom2.getBody())
+
+	@staticmethod
+	def is_ray(geom):
+		"""Is `geom` a ray?"""
+		return isinstance(geom, ode.GeomRay)
+
+	@staticmethod
+	def handle_ray_collision(ray, other_geom, contact):
+		ray_pos = ray.getPosition()
+		(pos, normal, depth, geom1, geom2) = contact.getContactGeomParams()
+		ray_contact = base.RayContactData(
+			ray, other_geom, ray_pos, normal, depth) # distance = depth
+		try:
+			ray.outer_object.set_last_contact(ray_contact)
+		except AttributeError:
+			print("`ray` has no attribute `outer_object` ")
+		except Exception:
+			print("Ray's encapsulating object's last_contact attribute could not be set")
 
 class Space(base.Space):
 	def __init__(self):
 		else:
 			self._inner_object.collide(args, callback)
 
+class OdeGeom(base.Geom):
 
-class Geom(base.Geom):
-	pass
+	def attach_body(self, body):
+		super(OdeGeom, self).attach_body(body)
+		self._inner_object.setBody(body.get_inner_object())
+
+	#===========================================================================
+	# GETTERS AND SETTERS
+	#===========================================================================
+
+	def get_position(self):
+		return self._inner_object.getPosition()
+
+	def get_rotation(self):
+		return self._inner_object.getRotation()
+
+	def set_position(self, pos):
+		self._inner_object.setPosition(pos)
+
+	def set_rotation(self, rot):
+		self._inner_object.setRotation(rot)
+
+class Ray(OdeGeom, base.Ray):
+
+	def __init__(self, space, length):
+		OdeGeom.__init__(self)
+		base.Ray.__init__(self, space, length)
+		self._inner_object = ode.GeomRay(space.get_inner_object(), length)
+		self._inner_object.outer_object = self # it is the encapsulating object
+
+	def get_length(self):
+		return self._inner_object.getLength()
+
+	def set_length(self, length):
+		self._inner_object.setLength(length)
+
+class Trimesh(OdeGeom, base.Trimesh):
+
+	def __init__(self, space, vertices, faces, center):
+		super(Trimesh, self).__init__()
+		tm_data = ode.TriMeshData()
+		tm_data.build(vertices, faces)
+		self._inner_object = ode.GeomTriMesh(tm_data, space.get_inner_object())
+		self._inner_object.setPosition(center)
+
+class OdeBasicShape(OdeGeom):
+	__metaclass__ = ABCMeta
+
+class Box(OdeBasicShape, base.Box):
+	"""Box shape, aligned along the X, Y and Z axii by default"""
+
+	def __init__(self, space, size):
+		super(Box, self).__init__(space, size)
+		self._inner_object = ode.GeomBox(space.get_inner_object(), lengths=size)
+
+class Sphere(OdeBasicShape, base.Sphere):
+	"""Spherical shape"""
+
+	def __init__(self, space, radius):
+		super(Sphere, self).__init__(space, radius)
+		self._inner_object = ode.GeomSphere(space.get_inner_object(), radius)
+
+class Capsule(OdeBasicShape, base.Capsule):
+	"""Capsule shape, aligned along the Z-axis by default"""
+
+	def __init__(self, space, length, radius):
+		# GeomCCylinder: Capped Cylinder
+		super(Capsule, self).__init__(space, length, radius)
+		self._inner_object = ode.GeomCCylinder(
+			space.get_inner_object(), radius, length)
+
+class Cylinder(OdeBasicShape, base.Cylinder):
+	"""Cylinder shape, aligned along the Z-axis by default"""
+
+	def __init__(self, space, length, radius):
+		super(Cylinder, self).__init__(space, length, radius)
+		self._inner_object = ode.GeomCylinder(
+			space.get_inner_object(), radius, length)
+
+class Plane(OdeBasicShape, base.Plane):
+	"""Plane, different from a box"""
+
+	def __init__(self, space, normal, dist):
+		super(Plane, self).__init__(space, normal, dist)
+		self._inner_object = ode.GeomPlane(
+			space.get_inner_object(), normal, dist)
+
+class Cone(OdeBasicShape, base.Cone):
+	"""Cone"""
+
+	def __init__(self):
+		super(Cone, self).__init__()
+		raise exc.ArsError("Not available in ODE")
 
 class NearCallbackArgs(base.NearCallbackArgs):
 	pass

File ars/model/collision/base.py

View file
  • Ignore whitespace
 
 from abc import ABCMeta, abstractmethod
 
+import ars.exceptions as exc
+
 class Engine:
 	__metaclass__ = ABCMeta
 
 #	def __init__(self):
 #		raise NotImplementedError()
 
-class Geom:
-	__metaclass__ = ABCMeta
-
-	@abstractmethod
-	def __init__(self):
-		raise NotImplementedError()
-
 class Space:
 	__metaclass__ = ABCMeta
 
 	@abstractmethod
 	def collide(self, args, callback):
 		raise NotImplementedError()
-	
+
+class Geom:
+	"""Encapsules a geometry object"""
+	__metaclass__ = ABCMeta
+
+	@abstractmethod
+	def __init__(self):
+		self._inner_object = None
+		self._attached_body = None
+
+	@abstractmethod
+	def attach_body(self, body):
+		self._attached_body = body
+
+	#===========================================================================
+	# GETTERS AND SETTERS
+	#===========================================================================
+	def get_inner_object(self):
+		return self._inner_object
+
+	def get_attached_body(self):
+		return self._attached_body
+
+	@abstractmethod
+	def get_position(self):
+		pass
+
+	@abstractmethod
+	def get_rotation(self):
+		pass
+
+	@abstractmethod
+	def set_position(self, pos):
+		pass
+
+	@abstractmethod
+	def set_rotation(self, rot):
+		pass
+
+class Ray(Geom):
+	"""
+	Ray aligned along the Z-axis by default.
+	"A ray is different from all the other geom classes in that it does not
+	represent a solid object. It is an infinitely thin line that starts from
+	the geom's position and	extends in the direction of the geom's local
+	Z-axis." (ODE Wiki Manual)
+	"""
+	__metaclass__ = ABCMeta
+
+	@abstractmethod
+	def __init__(self, space, length):
+		super(Ray, self).__init__()
+		self._last_contact = None
+		self._closer_contact = None
+
+	@abstractmethod
+	def get_length(self):
+		pass
+
+	@abstractmethod
+	def set_length(self, length):
+		pass
+
+	def get_last_contact(self):
+		"""Returns the contact object corresponding to the last collision of
+		the ray with another geom. Note than in each simulation step, several
+		collisions may occur, one for each intersection geom (in ODE).
+		The object returned may or may not be the same returned by
+		`get_closer_contact`.
+		"""
+		return self._last_contact
+
+	def get_closer_contact(self):
+		"""Returns the contact object corresponding to the collision closest to
+		the ray's origin.
+		It may or may not be the same object returned by `get_last_contact`.
+		"""
+		return self._closer_contact
+
+	def set_last_contact(self, last_contact):
+		"""Sets the last contact object with which the ray collided. It also
+		checks if `last_contact` is closer than the previously existing one.
+		The result can be obtained with the `get_closer_contact` method.
+		"""
+		if self._last_contact is None:
+			self._closer_contact = last_contact
+		else:
+			if self._last_contact.depth > last_contact.depth:
+				self._closer_contact = last_contact
+		self._last_contact = last_contact
+
+	def clear_last_contact(self):
+		self._last_contact = None
+
+	def clear_closer_contact(self):
+		self._closer_contact = None
+
+	def clear_contacts(self):
+		self.clear_last_contact()
+		self.clear_closer_contact()
+
+class Trimesh(Geom):
+
+	@abstractmethod
+	def __init__(self, space, vertices, faces, center):
+		super(Trimesh, self).__init__()
+
+	@staticmethod
+	def get_heightfield_faces(num_x, num_z):
+		"""Creates the faces corresponding to a heightfield size 'num_x' by
+		'num_z'. Faces are triangular, so each is composed by 3 vertices."""
+
+		# index of each square is calculated because it is needed to define faces
+		indices = []
+
+		for x in range(num_x):
+			indices_x = []
+			for z in range(num_z):
+				indices_x.insert(z, num_z * x + z)
+			indices.insert(x, indices_x)
+
+		# faces = [(1a,1b,1c), (2a,2b,2c), ...]
+		faces = []
+
+		for x in range(num_x - 1):
+			for z in range(num_z - 1):
+
+				zero = indices[x][z] # top-left corner
+				one = indices[x][z + 1] # bottom-left
+				two = indices[x + 1][z] # top-right
+				three = indices[x + 1][z + 1] # bottom-right
+
+				# there are two face types for each square
+				# contiguous squares must have different face types
+				face_type = zero
+				if num_z % 2 == 0:
+					face_type += 1
+
+				# there are 2 faces per square
+				if face_type % 2 == 0:
+					face1 = (zero, three, two)
+					face2 = (zero, one, three)
+				else:
+					face1 = (zero, one, two)
+					face2 = (one, three, two)
+
+				faces.append(face1)
+				faces.append(face2)
+
+		return faces
+
+	@staticmethod
+	def swap_faces_indices(faces):
+		"""Faces had to change their indices to work with ODE. With the initial
+		get_faces, the normal to the triangle defined by the 3 vertices pointed
+		(following the right-hand rule) downwards. Swapping the third with the
+		first index, now the triangle normal pointed upwards."""
+
+		new_faces = []
+		for face in faces:
+			new_faces.append((face[2], face[1], face[0]))
+		return new_faces
+
+	#===========================================================================
+	# def attach_body(self, body):
+	#	raise exc.ArsError('Trimesh shapes are not yet allowed to have a body attached')
+	#
+	# def get_attached_body(self):
+	#	raise exc.ArsError('Trimesh shapes are not yet allowed to have a body attached')
+	#===========================================================================
+
+
+class Terrain(Geom):
+	__metaclass__ = ABCMeta
+
+#===============================================================================
+# Basic Shapes
+#===============================================================================
+
+class BasicShape(Geom):
+	"""Abstract class from whom every solid object's shape derive"""
+	__metaclass__ = ABCMeta
+
+class Box(BasicShape):
+	"""Box shape, aligned along the X, Y and Z axii by default"""
+	__metaclass__ = ABCMeta
+
+	@abstractmethod
+	def __init__(self, space, size):
+		super(Box, self).__init__()
+
+class Sphere(BasicShape):
+	"""Spherical shape"""
+	__metaclass__ = ABCMeta
+
+	@abstractmethod
+	def __init__(self, space, radius):
+		super(Sphere, self).__init__()
+
+class Capsule(BasicShape):
+	"""Capsule shape, aligned along the Z-axis by default"""
+	__metaclass__ = ABCMeta
+
+	@abstractmethod
+	def __init__(self, space, length, radius):
+		super(Capsule, self).__init__()
+
+class Cylinder(BasicShape):
+	"""Cylinder shape, aligned along the Z-axis by default"""
+	__metaclass__ = ABCMeta
+
+	@abstractmethod
+	def __init__(self, space, length, radius):
+		super(Cylinder, self).__init__()
+
+class Plane(BasicShape):
+	"""Plane, different from a box"""
+	__metaclass__ = ABCMeta
+
+	@abstractmethod
+	def __init__(self, space, normal, dist):
+		super(Plane, self).__init__()
+
+class Cone(BasicShape):
+	"""Cone"""
+	__metaclass__ = ABCMeta
+
+	@abstractmethod
+	def __init__(self):
+		super(Cone, self).__init__()
+
 	#===========================================================================
 	# def add_ray(self, position, direction):
 	#	"""Adds a ray to the space, whose `_inner_object` will store it"""
 	#	shapes.Ray(self, position, direction) # no need for assignment
 	#===========================================================================
 
-class RayContact:
-	"""Contact information of a collision between a ray and a shape"""
+class RayContactData:
+	"""Contact information of a collision between `ray` and `shape`"""
 	
 	def __init__(self, ray=None, shape=None, position=None, normal=None, depth=None):
-		self._ray = ray
-		self._shape = shape
-		self._position = position # point at which the ray intersects the surface of the other geom
-		self._normal = normal # surface normal of the other geom at the contact point
-		self._depth = depth # distance from the start of the ray to the contact point
+		self.ray = ray
+		self.shape = shape
+		self.position = position # point at which the ray intersects the surface of the other shape/geom
+		self.normal = normal # surface normal of the other geom at the contact point
+		self.depth = depth # distance from the start of the ray to the contact point # TODO: is it true?
 
 class NearCallbackArgs:
 	"""Class to encapsulate args passed to the `near_callback` function"""

File ars/model/geometry/base.py

  • Ignore whitespace
-
-# Created on 2011.10.31
-#
-# @author: german
-
-from abc import ABCMeta, abstractmethod
-
-import ode
-
-import ars.exceptions as exc
-
-class Shape:
-	"""Encapsules a geometry object"""
-	__metaclass__ = ABCMeta
-
-	@abstractmethod
-	def __init__(self):
-		self._inner_object = None
-		self._attached_body = None
-
-	def attach_body(self, body):
-		self._inner_object.setBody(body.get_inner_object())
-		self._attached_body = body
-
-	def get_inner_object(self):
-		return self._inner_object
-
-	def get_attached_body(self):
-		return self._attached_body
-
-class Primitive(Shape):
-	__metaclass__ = ABCMeta
-
-class Trimesh(Shape):
-
-	def __init__(self, space, vertices, faces, center):
-		super(Trimesh, self).__init__()
-		tm_data = ode.TriMeshData()
-		tm_data.build(vertices, faces)
-		self._inner_object = ode.GeomTriMesh(tm_data, space.get_inner_object())
-		self._inner_object.setPosition(center)
-
-	@staticmethod
-	def get_heightfield_faces(num_x, num_z):
-		"""Creates the faces corresponding to a heightfield size 'num_x' by
-		'num_z'. Faces are triangular, so each is composed by 3 vertices."""
-
-		# index of each square is calculated because it is needed to define faces
-		indices = []
-
-		for x in range(num_x):
-			indices_x = []
-			for z in range(num_z):
-				indices_x.insert(z, num_z * x + z)
-			indices.insert(x, indices_x)
-
-		# faces = [(1a,1b,1c), (2a,2b,2c), ...]
-		faces = []
-
-		for x in range(num_x - 1):
-			for z in range(num_z - 1):
-
-				zero = indices[x][z] # top-left corner
-				one = indices[x][z + 1] # bottom-left
-				two = indices[x + 1][z] # top-right
-				three = indices[x + 1][z + 1] # bottom-right
-
-				# there are two face types for each square
-				# contiguous squares must have different face types
-				face_type = zero
-				if num_z % 2 == 0:
-					face_type += 1
-
-				# there are 2 faces per square
-				if face_type % 2 == 0:
-					face1 = (zero, three, two)
-					face2 = (zero, one, three)
-				else:
-					face1 = (zero, one, two)
-					face2 = (one, three, two)
-
-				faces.append(face1)
-				faces.append(face2)
-
-		return faces
-
-	@staticmethod
-	def swap_faces_indices(faces):
-		"""Faces had to change their indices to work with ODE. With the initial
-		get_faces, the normal to the triangle defined by the 3 vertices pointed
-		(following the right-hand rule) downwards. Swapping the third with the
-		first index, now the triangle normal pointed upwards."""
-
-		new_faces = []
-		for face in faces:
-			new_faces.append((face[2], face[1], face[0]))
-		return new_faces
-
-	#===========================================================================
-	# def attach_body(self, body):
-	#	raise exc.ArsError('Trimesh shapes are not yet allowed to have a body attached')
-	#
-	# def get_attached_body(self):
-	#	raise exc.ArsError('Trimesh shapes are not yet allowed to have a body attached')
-	#===========================================================================
-
-class Terrain(Shape):
-	__metaclass__ = ABCMeta
-
-
-class Box(Primitive):
-	"""Box shape, aligned along the X, Y and Z axii by default"""
-	def __init__(self, space, size):
-		super(Box, self).__init__()
-		self._inner_object = ode.GeomBox(space.get_inner_object(), lengths=size)
-
-class Sphere(Primitive):
-	"""Spherical shape"""
-	def __init__(self, space, radius):
-		super(Sphere, self).__init__()
-		self._inner_object = ode.GeomSphere(space.get_inner_object(), radius)
-
-class Capsule(Primitive):
-	"""Capsule shape, aligned along the Z-axis by default"""
-	def __init__(self, space, length, radius):
-		# GeomCCylinder != GeomCylinder
-		# GeomCCylinder: Capped Cylinder
-		super(Capsule, self).__init__()
-		self._inner_object = ode.GeomCCylinder(space.get_inner_object(), radius, length)
-
-class Cylinder(Primitive):
-	"""Cylinder shape, aligned along the Z-axis by default"""
-	def __init__(self, space, length, radius):
-		super(Cylinder, self).__init__()
-		self._inner_object = ode.GeomCylinder(space.get_inner_object(), radius, length)
-
-class Plane(Primitive):
-	"""Plane, different from a box"""
-	def __init__(self, space, normal, dist):
-		super(Plane, self).__init__()
-		self._inner_object = ode.GeomPlane(space.get_inner_object(), normal, dist)
-
-class Cone(Primitive):
-	"""Cone"""
-	def __init__(self):
-		super(Cone, self).__init__()
-		raise exc.ArsError("Not available in ODE")

File ars/model/physics/adapters.py

View file
  • Ignore whitespace
 import ode
 
 import ars.exceptions as exc
-import ars.model.geometry.base as shapes
+import ars.model.collision.adapters as shapes
 
 import base
 

File ars/model/simulator/__init__.py

View file
  • Ignore whitespace
 from ars.lib.pydispatch import dispatcher
 import ars.model.collision.adapters as coll
 #import ars.model.contrib.ragdoll as cb
-import ars.model.geometry.base as shapes
 import ars.model.physics.adapters as phs
 import ars.model.robot.joints as jo
 import ars.utilities.mathematical as mu
 	def get_time_step(self):
 		return self._DT / self._STEPS_PF
 
+	def get_collision_space(self):
+		return self._space
+
 	def add_axes(self):
 		gAxes = gp.Axes()
 		name = 'axes'
 		return self.add_object(SimulatedObject(name, actor=gFloor))
 
 	def add_trimesh_floor(self, vertices, faces, center=(0,0,0), color=(0.2,0.5,0.5)):
-		self._floor_geom = shapes.Trimesh(self._space, vertices, faces, center)
+		self._floor_geom = coll.Trimesh(self._space, vertices, faces, center)
 		gFloor = gp.Trimesh(vertices, faces, center)
 		gFloor.set_color(color)
 		name = "floor"

File bin/IROS/example4_sinusoidal_terrain.py

View file
  • Ignore whitespace
 from random import random
 
 from ars.lib.pydispatch import dispatcher
-from ars.model.geometry.base import Trimesh
+from ars.model.collision.base import Trimesh
 from ars.model.simulator import Simulation, SIM_PRE_STEP_SIGNAL
 import ars.utilities.mathematical as mut