Add trajectory function

Help to work with pandas multiindex dataframe
Will support lollipop plots

navipy/sensors/blendtraj.py

+"""
+"""
+import os
+import sys
+import argparse
+import inspect
+import pandas as pd
+
+
+def parser_blendtraj():
+    """ 
+    Parse argument for function blendtraj
+    """
+    # Create command line options
+    parser = argparse.ArgumentParser()
+    arghelp = 'Path to the environment (.blend) in which your agent lives'
+    defaultworld = pkg_resources.resource_filename(
+        'navipy', 'resources/twocylinders_world.blend')
+    parser.add_argument('--blender-world',
+                        type=str,
+                        default=defaultworld,
+                        help=arghelp)
+    arghelp = 'Command to run blender\n'
+    arghelp += 'If not provided, the script will try to find the command'
+    arghelp += " by using: shutil.which('blender')"
+    parser.add_argument('--blender-command',
+                        type=str,
+                        default=None,
+                        help=arghelp)
+    arghelp = 'To display some stuff \n'
+    arghelp += ' * -v print command \n'
+    arghelp += ' * -vv print also script'
+    parser.add_argument('-v', '--verbose',
+                        action='count',
+                        default=0,
+                        help=arghelp)
+    defaultfile = pkg_resources.resource_filename(
+        'navipy', 'resources/configs/BlenderRender.yaml')
+    arghelp = 'BlenderRender configuration files \n'
+    arghelp += 'by default: {}'.format(defaultfile)
+    parser.add_argument('--config',
+                        type=str,
+                        default=defaultfile,
+                        help=arghelp)
+    arghelp = 'Trajectory of the agent\n'
+    arghelp += 'pandas dataframe saved in hdf'
+    parser.add_argument('--trajectory',
+                        type=str,
+                        default=defaultfile,
+                        help=arghelp)
+
+
+
+def render_trajectory(traj_filename, output_path, imformat='.jpg'):
+    # Get extension to check that file is hdf
+    _, ext = os.path.splitext(traj_filename)
+    if ext in ['h5', 'hdf']:
+        trajdf = pd.read_hdf(traj_filename)
+    else:
+        raise IOError(
+            'File {} can not be read as a trajectory'.format(traj_filename))
+    
+
+def main():
+    """
+    Render a trajectory in a blender environment
+    """
+    # encoding for temporary file
+    encoding = 'utf-8'
+
+    # Fetch arguments
+    args = parser_blendtraj().parse_args()
+
+    # Create tempfile with testing code and then call blendnavipy
+    header = '# Generated by {}\n'.format(sys.argv[0])
+    with tempfile.NamedTemporaryFile() as tfile:
+        # Start of file
+        tfile.write(header.encode(encoding))
+        tfile.write('import unittest \n'.encode(encoding))
+        for line in inspect.getsourcelines(run)[0]:
+            tfile.write(line.encode(encoding))
+        tfile.write('\n\n'.encode(encoding))
+        tfile.write('try:\n'.encode(encoding))
+        tfile.write('     run("{}")\n'.format(args.start_dir).encode(encoding))
+        tfile.write('     sys.exit(0)\n'.encode(encoding))
+        tfile.write('except Exception:\n'.encode(encoding))
+        tfile.write('     sys.exit(1)\n'.encode(encoding))
+        tfile.seek(0)
+
+        command = 'blendnavipy --blender-world {} --python-script {}'
+        command = command.format(args.blender_world, tfile.name)
+        if args.blender_command is not None:
+            command += ' --blender-command {}'.format(args.blender_command)
+        for _ in range(args.verbose):
+            command += ' -v'
+        os.system(command)
+    
+if __name__ == "__main__":
+    # execute only if run as a script
+    main()

navipy/sensors/renderer.py

@@ -16,9 +16,97 @@ import yaml  # Used to load config files
 import pkg_resources
 from navipy.maths.homogeneous_transformations import compose_matrix
 import navipy.maths.constants as constants
+from navipy.tools.trajectory import Trajectory
 
 
-class BlenderRender():
+class AbstractRender():
+    """
+    List method to render on a grid or along a trajectory
+    """
+
+    def __init__(self):
+        pass
+
+    def render_trajectory(trajectory, outputfile, imformat='.jpg'):
+        pass
+
+    def render_ongrid(x, y, z, alpha_0=[0], alpha_1=[0], alpha_2=[0],
+                      q_0=None, q_1=None, q_2=None, q_3=None,
+                      rotconv='rzyx'):
+        if rotconv == 'quaternion':
+            if (q_0 is None) or \
+               (q_1 is None) or \
+               (q_2 is None) or \
+               (q_3 is None):
+                msg = 'With rotconv {}, q_0,q_1,q_2,q_3 can not be None'
+                msg = msg.format(rotconv)
+                raise ValueError(msg)
+        if not (isinstance(x, np.ndarray) or isinstance(x, list)):
+            raise TypeError('x must be list or np.array')
+        if not (isinstance(y, np.ndarray) or isinstance(y, list)):
+            raise TypeError('y must be list or np.array')
+        if not (isinstance(z, np.ndarray) or isinstance(z, list)):
+            raise TypeError('z must be list or np.array')
+
+        if rotconv == 'quaternion':
+            if not (isinstance(q_0, np.ndarray) or
+                    isinstance(q_0, list)):
+                raise TypeError('q_0 must be list or np.array')
+            if not (isinstance(q_1, np.ndarray) or
+                    isinstance(q_1, list)):
+                raise TypeError('q_1 must be list or np.array')
+            if not (isinstance(q_2, np.ndarray) or
+                    isinstance(q_2, list)):
+                raise TypeError('q_3 must be list or np.array')
+            if not (isinstance(q_3, np.ndarray) or
+                    isinstance(q_3, list)):
+                raise TypeError('q_3 must be list or np.array')
+            [mx, my, mz, mq0, mq1, mq2, mq3] = np.meshgrid(x,
+                                                           y,
+                                                           z,
+                                                           q_0,
+                                                           q_1,
+                                                           q_2,
+                                                           q_3)
+
+            mx = mx.flatten()
+            grid_point = Trajectory(rotconv, indeces=range(0, mx.shape[0]))
+            grid_point.x = mx
+            grid_point.y = my.flatten()
+            grid_point.z = mz.flatten()
+            grid_point.q0 = mq0.flatten()
+            grid_point.q1 = mq1.flatten()
+            grid_point.q2 = mq2.flatten()
+            grid_point.q3 = mq3.flatten()
+
+        else:
+            if not (isinstance(alpha_0, np.ndarray) or
+                    isinstance(alpha_0, list)):
+                raise TypeError('alpha_0 must be list or np.array')
+            if not (isinstance(alpha_1, np.ndarray) or
+                    isinstance(alpha_1, list)):
+                raise TypeError('alpha_1 must be list or np.array')
+            if not (isinstance(alpha_2, np.ndarray) or
+                    isinstance(alpha_2, list)):
+                raise TypeError('alpha_2 must be list or np.array')
+            [mx, my, mz, ma0, ma1, ma2] = np.meshgrid(x,
+                                                      y,
+                                                      z,
+                                                      alpha_0,
+                                                      alpha_1,
+                                                      alpha_2)
+
+            mx = mx.flatten()
+            grid_point = Trajectory(rotconv, indeces=range(0, mx.shape[0]))
+            grid_point.x = mx
+            grid_point.y = my.flatten()
+            grid_point.z = mz.flatten()
+            grid_point.alpha_0 = ma0.flatten()
+            grid_point.alpha_1 = ma1.flatten()
+            grid_point.alpha_2 = ma2.flatten()
+
+
+class BlenderRender(AbstractRender):
     """
     BlenderRender is a small class binding python with blender.
     With BlenderRender one can move the bee to a position, and render what
@@ -142,7 +230,7 @@ class BlenderRender():
             self.camera_gaussian_width = blendconfig['gaussian_width']
         else:
             raise KeyError(
-                    'Yaml config file should contain gaussian_width')
+                'Yaml config file should contain gaussian_width')
         # Load cycle for rendering
         if 'samples' in blendconfig.keys():
             self.camera_samples = blendconfig['samples']

navipy/tools/trajectory.py

+"""
+  Trajectory in navipy
+"""
+import pandas as pd
+import navipy.maths.constants as mconst
+
+
+class Trajectory(pd.DataFrame):
+    def __init__(self, rotconv, indeces):
+        if rotconv == 'quaternion':
+            index = pd.MultiIndex.from_tuples(
+                [('location', 'x'), ('location', 'y'),
+                 ('location', 'z'), (rotconv, 'q_0'),
+                    (rotconv, 'q_1'), (rotconv, 'q_2'),
+                 (rotconv, 'q_3')])
+        elif rotconv in mconst._AXES2TUPLE.keys():
+            index = pd.MultiIndex.from_tuples(
+                [('location', 'x'), ('location', 'y'),
+                 ('location', 'z'), (rotconv, 'alpha_0'),
+                    (rotconv, 'alpha_1'), (rotconv, 'alpha_2')])
+        else:
+            msg = 'convention for rotation {} is not suppored\n'
+            msg += msg.format(rotconv)
+            msg += 'the following convention are supported\n:'
+            for rconv in mconst._AXES2TUPLE.keys():
+                msg += '{}\n'.format(rconv)
+            msg += 'quaternion\n'
+            raise KeyError(msg)
+        super().__init__(index=indeces, columns=index)
+        self.__rotconv = rotconv
+
+    @property
+    def x(self):
+        return self.loc[:, ('location', 'x')]
+
+    @x.setter
+    def x(self, x):
+        self.loc[:, ('location', 'x')] = x
+
+    @property
+    def y(self):
+        return self.loc[:, ('location', 'y')]
+
+    @y.setter
+    def y(self, y):
+        self.loc[:, ('location', 'y')] = y
+
+    @property
+    def z(self):
+        return self.loc[:, ('location', 'z')]
+
+    @z.setter
+    def z(self, z):
+        self.loc[:, ('location', 'z')] = z
+
+    def __get_alpha_i(self, alphai):
+        if self.__rotconf != 'quaternion':
+            return self.loc[:, (self.__rotconf, 'alpha_{}'.format(alphai))]
+        else:
+            msg = 'alpha_{0:} does not exist for quaternion (try q_{0:})'
+            raise ValueError(msg.format(alphai))
+
+    def __set_alpha_i(self, alphai, val):
+        if self.__rotconf != 'quaternion':
+            self.loc[:, (self.__rotconf, 'alpha_{}'.format(alphai))] = val
+        else:
+            msg = 'alpha_{0:} does not exist for quaternion (try q_{0:})'
+            raise ValueError(msg.format(alphai))
+
+    @property
+    def alpha_0(self):
+        self.__get_alpha_i(0)
+
+    @alpha_0.setter
+    def alpha_0(self, alpha_0):
+        self.__set_alpha_i(0, alpha_0)
+
+    @property
+    def alpha_1(self):
+        self.__get_alpha_i(1)
+
+    @alpha_1.setter
+    def alpha_1(self, alpha_1):
+        self.__set_alpha_i(1, alpha_1)
+
+    @property
+    def alpha_2(self):
+        self.__get_alpha_i(2)
+
+    @alpha_2.setter
+    def alpha_2(self, alpha_2):
+        self.__set_alpha_i(2, alpha_2)
+
+    def __get_q_i(self, qi):
+        if self.__rotconf == 'quaternion':
+            return self.loc[:, (self.__rotconf, 'q_{}'.format(qi))]
+        else:
+            msg = 'q_{0:} does not exist for none quaternion (try alpha_{0:})'
+            raise ValueError(msg.format(qi))
+
+    def __set_q_i(self, qi, val):
+        if self.__rotconf != 'quaternion':
+            self.loc[:, (self.__rotconf, 'q_{}'.format(qi))] = val
+        else:
+            msg = 'q_{0:} does not exist for none quaternion (try alpha_{0:})'
+            raise ValueError(msg.format(qi))
+
+    @property
+    def q_0(self):
+        self.__get_q_i(0)
+
+    @q_0.setter
+    def q_0(self, q_0):
+        self.__set_q_i(0, q_0)
+
+    @property
+    def q_1(self):
+        self.__get_q_i(1)
+
+    @q_1.setter
+    def q_1(self, q_1):
+        self.__set_q_i(1, q_1)
+
+    @property
+    def q_2(self):
+        self.__get_q_i(2)
+
+    @q_2.setter
+    def q_2(self, q_2):
+        self.__set_q_i(2, q_2)
+
+    @property
+    def q_3(self):
+        self.__get_q_i(3)
+
+    @q_3.setter
+    def q_3(self, q_3):
+        self.__set_q_i(3, q_3)
+
+    def lollipops(self):
+        raise NameError('Not implemented')