def __init__(self, world, space, position): '''Yes, right now we're approximating the sub as a box Not taking any parameters because this is the literal Sub8 TODO: - Make the thruster list a parameter - Stop the sub from being able to fly (Only thrust if thruster_z < 0) See Annie for how the thrusters work ''' lx, ly, lz = 0.5588, 0.5588, 0.381 # Meters self.radius = max((lx, ly, lz)) * 0.5 # For spherical approximation volume = lx * ly * lz * 0.25 # self.mass = 32.75 # kg self.mass = 25.0 # kg density = self.mass / volume super(self.__class__, self).__init__(world, space, position, density, lx, ly, lz) self.truth_odom_pub = rospy.Publisher('truth/odom', Odometry, queue_size=1) self.thruster_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=2) self.imu_sensor_pub = rospy.Publisher('imu', Imu, queue_size=1) self.dvl_sensor_pub = rospy.Publisher('dvl', VelocityMeasurements, queue_size=1) self.thruster_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=2) self.thrust_dict = {} self.last_vel = np.array([0.0, 0.0, 0.0], dtype=np.float32) self.cur_vel = np.array([0.0, 0.0, 0.0], dtype=np.float32) self.space = space # Define 4 rays to implement a DVL sensor and the relative position of the sensor on the sub # TODO: Fix position of DVL self.dvl_position = np.array([0.0, 0.0, 0.0]) self.dvl_rays = [] self.dvl_rays.append(ode.GeomRay(None, 1e3)) self.dvl_rays.append(ode.GeomRay(None, 1e3)) self.dvl_rays.append(ode.GeomRay(None, 1e3)) self.dvl_rays.append(ode.GeomRay(None, 1e3)) self.dvl_ray_orientations = (np.array([0.866, -.5, -1]), np.array([0.866, .5, -1]), np.array([-0.866, .5, -1]), np.array([-0.866, -.5, -1])) # Make this a parameter # name, relative direction, relative position (COM) self.thruster_list = [ ("FLV", np.array([ 0.000, 0.0, -1]), np.array([ 0.1583, 0.16900, 0.0142])), # noqa ("FLL", np.array([-0.866, 0.5, 0]), np.array([ 0.2678, 0.27950, 0.0000])), # noqa ("FRV", np.array([ 0.000, 0.0, -1]), np.array([ 0.1583, -0.1690, 0.0142])), # noqa ("FRL", np.array([-0.866, -0.5, 0]), np.array([ 0.2678, -0.2795, 0.0000])), # noqa ("BLV", np.array([ 0.000, 0.0, 1]), np.array([-0.1583, 0.16900, 0.0142])), # noqa ("BLL", np.array([ 0.866, 0.5, 0]), np.array([-0.2678, 0.27950, 0.0000])), # noqa ("BRV", np.array([ 0.000, 0.0, 1]), np.array([-0.1583, -0.1690, 0.0142])), # noqa ("BRL", np.array([ 0.866, -0.5, 0]), np.array([-0.2678, -0.2795, 0.0000])), # noqa ] self.last_cmd_time = time() self.set_up_ros()
def create_ray(self, key, pos, rot, length=5.0): """ Create a ray geom. Arguments: key: key identifying the geom. pos: position to start the ray from. rot: list of pitch,roll,yaw in degrees (determines where it points) """ # Create body body = ode.Body(self.world) M = ode.Mass() M.setBox(0.1, 0.1, 0.1, 0.1) body.setMass(M) # Set parameters for drawing the body body.shape = "box" body.boxsize = (0.1, 0.1, 0.1) body.setPosition((pos[0], pos[1], pos[2])) geom = ode.GeomRay(self.space, length) geom.setPosition((pos)) #geom.setRotation(self.form_rotation(rot)) body.setRotation(self.form_rotation(rot)) geom.setBody(body) # Set min_dist for sensor. geom.min_dist = -1 # Append to the manager lists. self.bodies[key] = body self.geoms[key] = geom
def process_cutter_movement(self, location_start, location_end): # TODO: fix this workaround in the cutters shape defintions (or in ODE?) # for now we may only move from low x/y values to higher x/y values if (location_start.x > location_end.x) \ or (location_start.y > location_end.y): location_start, location_end = location_end, location_start cutter_body = ode.Body(self.world) cutter_shape, cutter_position_func = self.cutter.get_shape("ODE") self.space.add(cutter_shape) cutter_shape.space = self.space cutter_shape.setBody(cutter_body) cutter_position_func(location_start.x, location_start.y, location_start.z) cutter_shape.extend_shape(location_end.x - location_start.x, location_end.y - location_start.y, location_end.z - location_start.z) aabb = cutter_shape.getAABB() cutter_height = aabb[5] - aabb[4] # add a ray along the drill to work around an ODE bug in v0.11.1 # http://sourceforge.net/tracker/index.php?func=detail&aid=2973876&group_id=24884&atid=382799 currx, curry, currz = cutter_shape.getPosition() ray = ode.GeomRay(self.space, cutter_height) ray.set((currx, curry, aabb[5]), (0.0, 0.0, -1.0)) # check for collisions all_cutter_shapes = [cutter_shape] + cutter_shape.children + [ray] def check_collision(item): for one_cutter_shape in all_cutter_shapes: if ode.collide(item, one_cutter_shape): return True return False for index in range(len(self.boxes)): if check_collision(self.boxes[index]): self.shrink_box_avoiding_collision(index, check_collision)
def loadModel(self, filename): mesh = loadObj(filename) body = ode.Body(self.world) self.model = ode.GeomTriMesh(mesh, self.space[0]) self.model.setBody(body) # rotate so that z becomes top self.addGeom(self.model, "model") self.model.setQuaternion( (0.7071067811865476, 0.7071067811865475, 0, 0)) body = ode.Body(self.world) ray = ode.GeomRay(self.space[0], 10000) ray.setBody(body) ray.set((0, 0, 1), (0, 0, 100000)) self.addGeom(ray, "XXXXXXX") self.GetProperty(ray).SetColor(1, 0, 0) self.GetProperty(ray).SetLineWidth(30) print(self.GetProperty(ray)) self.update()
def handleReflectionForRays(self, rayContacts): # determine if we are making more reflections, and if there are intersections with objects newRayList = [] newRaySpace = ode.HashSpace() intersectionList = defaultdict(list) for ray, contactList in rayContacts.items(): for contact in contactList: pos, normal, depth, g1, g2 = contact.getContactGeomParams() # is it in our object list? then we need to record this intersection obj = self.environment.getObjectFromGeom(g2) sensor = self.findSensorForObject(obj) if sensor is not None: # don't be intersected by rays coming from us... if ray.getPosition() != sensor.getPosition(): intersectionList[sensor].append(ray) else: try: if obj.isObstacle: # reflect reflectDir = normal reflectFrom = pos newLength = ray.getLength() - depth newRay = ode.GeomRay(newRaySpace, newLength) newRay.set(reflectFrom, reflectDir) newRay.intensity = ray.intensity newRayList.append(newRay) except AttributeError: pass # is it an obstacle? return newRayList, newRaySpace, intersectionList
def _get_rays_for_geom(self, geom): """ TODO: this is necessary due to a bug in the trimesh collision detection code of ODE v0.11.1. Remove this as soon as the code is fixed. http://sourceforge.net/tracker/index.php?func=detail&aid=2973876&group_id=24884&atid=382799 """ minz, maxz = geom.getAABB()[-2:] currx, curry = geom.getPosition()[0:2] ray = ode.GeomRay(self._space, maxz - minz) ray.set((currx, curry, maxz), (0.0, 0.0, -1.0)) return [ray]
def _parseGeomRay(self, attrs): def start(name, attrs): if (name == 'ext'): pass else: raise errors.ChildError('ray', name) def end(name): if (name == 'ray'): self._parser.pop() length = float(attrs['length']) self.setODEObject(ode.GeomRay(self._space, length)) self._parser.push(startElement=start, endElement=end)
def createRaysForObject(self, origin, emissionTimes, now, space): rayList = [] # for now, just generate 8 rays: # radiating to cube corners directions = [(1, 1, 1), (-1, -1, -1), (1, 1, -1), (-1, -1, 1), (1, -1, 1), (-1, 1, -1), (-1, 1, 1), (1, -1, -1)] for et in emissionTimes: for d in directions: (t, pw) = et radius = self.speed * (now - t) newRay = ode.GeomRay(space, radius) newRay.set(origin, d) newRay.intensity = pw #newRay = FieldRay(radius, origin, d, pw, space) rayList.append(newRay) return rayList
def initGPS(self, gps_ops_file, lat=0, lon=0, ele=0, pressure=0, horizon="0:0"): #'-0:32'): self.observer.lat = str(lat) #np.deg2rad(lat) self.observer.long = str(lon) #np.deg2rad(lon) self.observer.elevation = ele self.observer.pressure = pressure self.observer.horizon = horizon f = open(gps_ops_file) l1 = f.readline() while l1: l2 = f.readline() l3 = f.readline() # ephem.readtle() creates a PyEphem Body object from that TLE sat = ephem.readtle(l1, l2, l3) self.satellites[sat.name] = { "ephem": sat, "visible": False, "position": (0, 0, 0), "ray": None } l1 = f.readline() f.close() for name in self.satellites.keys(): body = ode.Body(self.world) ray = ode.GeomRay(self.space[0], 10000) ray.setBody(body) self.addGeom(ray, name) self.GetProperty(ray).SetColor(1, 0, 0) self.GetProperty(ray).SetLineWidth(3) self.GetActor(ray).SetVisibility(True) self.satellites[name]["ray"] = ray
def __init__(self, world, space, position): '''Yes, right now we're approximating the sub as a box Not taking many parameters because this is the literal Sub8 thruster_layout: A list of tuples, each tuple formed as... (thruster_name, relative_direction, position_wrt_sub_center-of-mass) All of these in the FLU body frame TODO: - Independent publishing rates See Annie for how the thrusters work ''' lx, ly, lz = 0.5588, 0.5588, 0.381 # Meters self.radius = max((lx, ly, lz)) * 0.32 # For spherical approximation self.mass = 32.75 # kg density = self.mass / (lx * ly * lz) # This is a leaky fix super(self.__class__, self).__init__(world, space, position, density, lx, ly, lz) self.truth_odom_pub = rospy.Publisher('truth/odom', Odometry, queue_size=1) self.imu_sensor_pub = rospy.Publisher('imu', Imu, queue_size=1) self.dvl_sensor_pub = rospy.Publisher('dvl', VelocityMeasurements, queue_size=1) self.thruster_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=2) self.thrust_dict = {} self.last_force = (0.0, 0.0, 0.0) self.last_vel = np.array([0.0, 0.0, 0.0], dtype=np.float32) self.cur_vel = np.array([0.0, 0.0, 0.0], dtype=np.float32) self.space = space self.g = np.array(world.getGravity()) # TODO: Fix position of DVL, IMU # We assume that the DVL and IMU are xyz-FLU aligned (No rotation w.r.t sub) self.dvl_position = np.array([0.0, 0.0, 0.0]) self.imu_position = np.array([0.0, 0.0, 0.0]) # Define 4 rays to implement a DVL sensor and the relative position of the sensor on the sub self.dvl_ray_geoms = ( ode.GeomRay(None, 1e3), ode.GeomRay(None, 1e3), ode.GeomRay(None, 1e3), ode.GeomRay(None, 1e3), ) self.dvl_ray_orientations = ( np.array([+0.866, -.5, -1]), np.array([+0.866, +.5, -1]), np.array([-0.866, +.5, -1]), np.array([-0.866, -.5, -1]) ) self.dvl_rays = zip(self.dvl_ray_geoms, self.dvl_ray_orientations) # Make this a parameter # name, relative direction, relative position (COM) self.thruster_list = [ ("FLV", np.array([+0.000, +0.0, -1.]), np.array([+0.1583, +0.1690, +0.0142])), ("FLL", np.array([-0.866, +0.5, +0.]), np.array([+0.2678, +0.2795, +0.0000])), ("FRV", np.array([+0.000, +0.0, -1.]), np.array([+0.1583, -0.1690, +0.0142])), ("FRL", np.array([-0.866, -0.5, +0.]), np.array([+0.2678, -0.2795, +0.0000])), ("BLV", np.array([+0.000, +0.0, +1.]), np.array([-0.1583, +0.1690, +0.0142])), ("BLL", np.array([+0.866, +0.5, +0.]), np.array([-0.2678, +0.2795, +0.0000])), ("BRV", np.array([+0.000, +0.0, +1.]), np.array([-0.1583, -0.1690, +0.0142])), ("BRL", np.array([+0.866, -0.5, +0.]), np.array([-0.2678, -0.2795, +0.0000])), ] self.thrust_range = (-70, 100) self.last_cmd_time = rospy.Time.now() self.set_up_ros()
def __init__(self): Satellites.__init__(self) body = ode.Body(self.world) self.scan_ray = ode.GeomRay(self.space[0], 10000) self.scan_ray.setBody(body)