From 3712655b84ae8ce7110214febf85c9ef481660ae Mon Sep 17 00:00:00 2001
From: "Olivier J.N. Bertrand" <bolirev@hotmail.com>
Date: Sat, 9 Dec 2017 18:22:33 +0100
Subject: [PATCH] Add moving function and their test
---
navipy/database/__init__.py | 18 ++++
navipy/moving/agent.py | 187 +++++++++++++++++++++++++++++-------
navipy/moving/maths.py | 35 ++++---
3 files changed, 189 insertions(+), 51 deletions(-)
diff --git a/navipy/database/__init__.py b/navipy/database/__init__.py
index ddd6ae2..7db665c 100644
--- a/navipy/database/__init__.py
+++ b/navipy/database/__init__.py
@@ -306,6 +306,24 @@ database
posorient.set_index('id', inplace=True)
return posorient
+ def read_posorient(self, posorient=None, rowid=None):
+ assert (posorient is None) or (rowid is None),\
+ 'posorient and rowid can not be both None'
+ if posorient is not None:
+ rowid = self.get_posid(posorient)
+ # Read images
+ tablename = 'position_orientation'
+ toreturn = pd.read_sql_query(
+ """
+ SELECT *
+ FROM {}
+ WHERE (rowid={})
+ """.format(tablename, rowid), self.db)
+ toreturn = toreturn.loc[0, :]
+ toreturn.name = toreturn.id
+ toreturn.drop('id')
+ return toreturn
+
def read_image(self, posorient=None, rowid=None):
"""Read an image at a given position-orientation or given id of row in the \
database.
diff --git a/navipy/moving/agent.py b/navipy/moving/agent.py
index ced17a7..5561fde 100644
--- a/navipy/moving/agent.py
+++ b/navipy/moving/agent.py
@@ -3,32 +3,38 @@
"""
import numpy as np
import pandas as pd
+import networkx as nx
from pandas.api.types import is_numeric_dtype
from navipy.database import DataBaseLoad
-from .maths import next_pos, \
- closest_pos, \
- closest_pos_memory_friendly, \
- __mode_moves_supported
+import navipy.moving.maths as navimomath
class AbstractAgent():
- # Define a default mode_of_motion
- __mode_move = {'mode': 'on_cubic_grid',
- 'param': {'grid_spacing': 1}}
def __init__(self,
database,
memory_friendly=False):
if isinstance(database, DataBaseLoad):
- self.__db = database
+ self.db = database
else:
raise TypeError('database should be of type DataBaseLoad')
if memory_friendly:
self.__posorients = None
else:
- self.__posorients = self.__db.get_posorients()
+ self.__posorients = self.db.get_posorients()
+ # set mode of motion
+ mode_move = {'mode': 'on_cubic_grid',
+ 'param': {'grid_spacing': 1}}
+ self.mode_of_motion = mode_move
+
+ @property
+ def posorients(self):
+ toreturn = self.__posorients
+ if toreturn is not None:
+ toreturn = toreturn.copy()
+ return toreturn
@property
def mode_of_motion(self):
@@ -36,7 +42,8 @@ class AbstractAgent():
"""
toreturn = self.__mode_move
toreturn['describe'] = \
- __mode_moves_supported[self.__mode_move['mode']]['describe']
+ navimomath.mode_moves_supported()[
+ self.__mode_move['mode']]['describe']
return toreturn
@mode_of_motion.setter
@@ -50,8 +57,9 @@ class AbstractAgent():
raise KeyError("'mode' is not a key of mode")
if 'param' not in mode:
raise KeyError("'param' is not a key of mode")
- if mode['mode'] in __mode_moves_supported.keys:
- for param in __mode_moves_supported[mode['mode']]['param']:
+ if mode['mode'] in navimomath.mode_moves_supported().keys():
+ for param in navimomath.mode_moves_supported()[
+ mode['mode']]['param']:
if param not in mode['param']:
raise KeyError(
"'{}' is not in mode['param']".format(param))
@@ -61,40 +69,50 @@ class AbstractAgent():
def abstractmove(self, posorients_vel):
- # NEED TO CHECK posorients_vel
-
+ if isinstance(posorients_vel, pd.Series) is False:
+ raise TypeError('posorients_vel should be a pandas Series')
+ for col in ['x', 'y', 'z', 'yaw', 'pitch', 'roll',
+ 'dx', 'dy', 'dz', 'dyaw', 'dpitch', 'droll']:
+ if col not in posorients_vel.index:
+ raise KeyError(
+ 'posorients_vel should have {} as index'.format(col))
# Compute the next position
- posorients_vel = next_pos(posorients_vel,
- move_mode=self.__mode_move['mode'],
- move_param=self.__mode_move['param'])
+ posorients_vel = navimomath.next_pos(
+ posorients_vel,
+ move_mode=self.__mode_move['mode'],
+ move_param=self.__mode_move['param'])
# Compute the closest possible position
if posorients_vel is None:
posorients_vel[['x', 'y', 'z',
'yaw', 'pitch', 'roll']] = \
- closest_pos_memory_friendly(posorients_vel, self.__db)
+ navimomath.closest_pos_memory_friendly(
+ posorients_vel,
+ self.db)
else:
posorients_vel[['x', 'y', 'z',
'yaw', 'pitch', 'roll']] = \
- closest_pos(posorients_vel, self.__posorients)
+ navimomath.closest_pos(
+ posorients_vel,
+ self.__posorients)
return posorients_vel
class Single(AbstractAgent):
- __posorientvel = pd.Series(['x', 'y', 'z',
- 'yaw', 'pitch', 'roll',
- 'dx', 'dy', 'dy',
- 'dyaw', 'dpitch', 'droll'],
- dtype=np.float,
- data=np.nan)
- # Define a list of supported mode in a dictionary
- # key mode, val is list of required parameters
def __init__(self,
database,
initial_condition,
memory_friendly=False):
super().__init__(database, memory_friendly)
+
+ self.__posorientvel = pd.Series(
+ index=['x', 'y', 'z',
+ 'yaw', 'pitch', 'roll',
+ 'dx', 'dy', 'dz',
+ 'dyaw', 'dpitch', 'droll'],
+ dtype=np.float)
+
if isinstance(initial_condition, pd.Series):
if is_numeric_dtype(initial_condition):
for key in self.__posorientvel.index:
@@ -154,13 +172,16 @@ class Single(AbstractAgent):
raise TypeError('vel should be a pandas Series')
-def Multi(AbstractAgent):
+class Multi(AbstractAgent):
- def __init__(self, database,
- memory_friendly=False):
- super().__init__(database, memory_friendly)
+ def __init__(self, database):
+ super().__init__(database, False)
# Init the graph
- self.__graph = None
+ self.__graph = nx.DiGraph()
+ for row_id, posor in self.db.get_posorients().iterrows():
+ posor.name = row_id
+ self.__graph.add_node(row_id,
+ posorient=posor)
@property
def graph(self):
@@ -168,12 +189,104 @@ def Multi(AbstractAgent):
@graph.setter
def graph(self, graph):
- # Check that graph is properly formatted
- pass
+ if isinstance(graph, nx.DiGraph) is False:
+ raise TypeError('graph is not a nx.DiGraph')
+ self.__graph = graph.copy()
+ self.check_graph()
def build_graph(self, callback_function):
# Build a graph with luises code
- pass
+ for node in self.__graph.nodes:
+ posorient = self.__graph[node]['posorient']
+ next_node = callback_function(posorient)
+ if next_node not in self.__graph.nodes():
+ raise ValueError('Node does not exist')
+ else:
+ self.__graph.add_edge(node, next_node)
+ self.check_graph()
+
+ def check_graph(self):
+ self.check_single_target()
+
+ def check_single_target(self):
+ for node in self.__graph.nodes:
+ # not connected -> loop not ran
+ for count, _ in enumerate(self.__graph.neighbors(node)):
+ # count == 0 -> connected to one node
+ # count == 1 -> connected to two nodes
+ if count > 0:
+ raise ValueError(
+ 'Node {} leads to several locations'.format(node))
+
+ def find_attractors(self):
+ """Return a list of node going to each attractor in a graph
+ """
+ attractors = list()
+ for attractor in nx.attracting_components(self.__graph):
+ att = dict()
+ att['attractor'] = attractor
+ attractors.append(att)
+ return attractors
+
+ def find_attractors_sources(self, attractors=None):
+ """Find all sources going to each attractors
+ """
+ if attractors is None:
+ attractors = self.find_attractors()
+
+ if isinstance(attractors, list) is False:
+ raise TypeError('Attractors should be a list of dict')
+ elif len(attractors) == 0:
+ raise ValueError('No attractors found')
+
+ # Check attractor
+ for att in attractors:
+ keyatt = att.keys()
+ if 'attractor' not in keyatt:
+ raise ValueError(
+ 'Each attractors should contain the key attractor')
+
+ # Calculate connection
+ for att_i, att in enumerate(attractors):
+
+ # [0] because one node of the attractor is enough
+ # all other node of the attractor are connected to this one
+ target = list(att['attractor'])[0]
+ attractors[att_i]['paths'] = \
+ nx.shortest_path(self.graph, target=target)
+ attractors[att_i]['sources'] = \
+ list(attractors[att_i]['paths'].keys())
+ return attractors
+
+ def catchment_area(self, attractors=None):
+ """Return the catchment area for attractors
+ """
+ if attractors is None:
+ attractors = self.find_attractors_sources()
+
+ if isinstance(attractors, list) is False:
+ raise TypeError('Attractors should be a list of dict')
+ elif len(attractors) == 0:
+ raise ValueError('No attractors found')
+
+ # Check attractor
+ for att in attractors:
+ keyatt = att.keys()
+ if 'sources' not in keyatt:
+ raise ValueError(
+ 'Each attractors should contains a list of sources')
+
+ return [len(att['sources']) for att in attractors]
+
+ def reach_goals(self, goals):
+ """ Return all paths to the goals """
+ return nx.shortest_path(self.__graph, target=goals)
- def reach_goals():
- pass
+ def neighboring_nodes(self, target):
+ """ Return the nodes going to the target """
+ # Reverse graph because nx.neighbors give the end node
+ # and we want to find the start node going to target
+ # not where target goes.
+ tmpgraph = self.__graph.reverse(copy=True)
+ neighbors = tmpgraph.neighbors(target)
+ return neighbors
diff --git a/navipy/moving/maths.py b/navipy/moving/maths.py
index 4487a37..49b3471 100644
--- a/navipy/moving/maths.py
+++ b/navipy/moving/maths.py
@@ -5,16 +5,18 @@ geometry, and predefined grids shapes
import numpy as np
import pandas as pd
-__mode_moves_supported = {
- 'on_cubic_grid': {
- 'param':
+
+def mode_moves_supported():
+ return {
+ 'on_cubic_grid': {
+ 'param':
['grid_spacing'],
'describe':
"Agent restricted to move on a grid"},
- 'free_run': {
- 'param': [],
- 'describe':
- "Freely moving agent, pos(t+dt)=pos+speed (dt=1)"}}
+ 'free_run': {
+ 'param': [],
+ 'describe':
+ "Freely moving agent, pos(t+dt)=pos+speed (dt=1)"}}
def next_pos(motion_vec, move_mode, move_param=None):
@@ -28,23 +30,28 @@ def next_pos(motion_vec, move_mode, move_param=None):
..todo: add literal include for supported_grid_mode
"""
- assert isinstance(motion_vec, pd.Series),\
- 'motion vector must be a pandas Series'
- assert move_mode in __mode_moves_supported,\
- 'move mode must is not supported {}'.format(move_mode)
+ if isinstance(motion_vec, pd.Series) is False:
+ raise TypeError('motion vector must be a pandas Series')
+ if move_mode not in mode_moves_supported().keys():
+ raise KeyError(
+ 'move mode must is not supported {}'.format(move_mode))
speed = motion_vec.loc[['dx', 'dy', 'dz']]
- position = motion_vec.loc[['x', 'y', 'z']]
if move_mode == 'on_cubic_grid':
grid_spacing = move_param['grid_spacing']
- speed /= np.linalg.norm(speed)
+ if np.linalg.norm(speed) > 0:
+ speed /= np.linalg.norm(speed)
scaling = grid_spacing / (2 * np.sin(np.pi / 8))
elif move_mode is 'free_run':
scaling = 1 # <=> dt = 1, user need to scale speed in dt units
else:
raise ValueError('grid_mode is not supported')
- return position + speed.rename({'dx': 'x', 'dy': 'y', 'dz': 'z'}) * scaling
+ toreturn = motion_vec
+ toreturn.loc[['x', 'y', 'z']] += speed.rename({'dx': 'x',
+ 'dy': 'y',
+ 'dz': 'z'}) * scaling
+ return toreturn
def closest_pos(pos, positions):
--
GitLab