diff --git a/src/modules/systemlib/mixer/geoms/quad_x.toml b/src/modules/systemlib/mixer/geoms/quad_x.toml index 8e7fc4be91..bb76f8bc9b 100644 --- a/src/modules/systemlib/mixer/geoms/quad_x.toml +++ b/src/modules/systemlib/mixer/geoms/quad_x.toml @@ -13,7 +13,7 @@ Cm = 0.05 [[rotors]] name = "front_right" -position = [0.707107, 0.707107, 0.0] +position = [0.707107, 0.707107]#, 0.0] direction = "CCW" [[rotors]] diff --git a/src/modules/systemlib/mixer/geoms/tools/px_generate_mixers.py b/src/modules/systemlib/mixer/geoms/tools/px_generate_mixers.py index 46ed66bd3a..0b42276a2b 100644 --- a/src/modules/systemlib/mixer/geoms/tools/px_generate_mixers.py +++ b/src/modules/systemlib/mixer/geoms/tools/px_generate_mixers.py @@ -68,8 +68,6 @@ def parse_geom_toml(filename): Parses toml geometry file and returns a dictionary with curated list of rotors ''' - import toml - # Load toml file d = toml.load(filename) @@ -79,6 +77,16 @@ def parse_geom_toml(filename): else: default = {} + # Check info section + if 'info' not in d: + raise AttributeError('{}: Error, missing info section'.format(filename)) + + # Check info section + for field in ['name', 'key', 'description']: + if field not in d['info']: + raise AttributeError('{}: Error, unspecified info field "{}"'.format(filename, field)) + + # Convert rotors rotor_list = [] if 'rotors' in d: @@ -89,20 +97,28 @@ def parse_geom_toml(filename): if field in default: r[field] = default[field] else: - print(filename + ': Error, unspecified field ' + field + ' for rotor ' + r['name']) + raise AttributeError('{}: Error, unspecified field "{}" for rotor "{}"' + .format(filename, field, r['name'])) - # Check fields + # Check direction field r['direction'] = r['direction'].upper() if r['direction'] not in ['CW', 'CCW']: - print(filename + ': Error, invalid direction value "' + r['direction'] + '" for rotor ' + r['name']) + raise AttributeError('{}: Error, invalid direction value "{}" for rotor "{}"' + .format(filename, r['direction'], r['name'])) + # Check vector3 fields + for field in ['position', 'axis']: + if len(r[field]) != 3: + raise AttributeError('{}: Error, field "{}" for rotor "{}"' + .format(filename, field, r['name']) + + ' must be an array of length 3') # Add rotor to list rotor_list.append(r) # Clean dictionary geom = {'info': d['info'], - 'rotors': rotor_list} + 'rotors': rotor_list} return geom @@ -111,7 +127,7 @@ def torque_matrix(center, axis, dirs, Ct, Cm): Compute torque generated by rotors ''' # normalize rotor axis - ax = axis / np.linalg.norm(axis, axis=1)[:,np.newaxis] + ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis] torque = Ct * np.cross(center, ax) - Cm * ax * dirs return torque @@ -124,7 +140,7 @@ def geom_to_torque_matrix(geom): Am = torque_matrix(center=np.array([rotor['position'] for rotor in geom['rotors']]), axis=np.array([rotor['axis'] for rotor in geom['rotors']]), dirs=np.array([[1.0 if rotor['direction'] == 'CCW' else -1.0] - for rotor in geom['rotors']]), + for rotor in geom['rotors']]), Ct=np.array([[rotor['Ct']] for rotor in geom['rotors']]), Cm=np.array([[rotor['Cm']] for rotor in geom['rotors']])).T return Am @@ -134,7 +150,7 @@ def thrust_matrix(axis, Ct): Compute thrust generated by rotors ''' # Normalize rotor axis - ax = axis / np.linalg.norm(axis, axis=1)[:,np.newaxis] + ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis] thrust = Ct * ax return thrust @@ -175,8 +191,8 @@ def normalize_mix_px4(B): Normalize mix for PX4 This is for compatibility only and should ideally not be used ''' - B_norm = np.linalg.norm(B,axis=0) - B_max = np.abs(B).max(axis=0) + B_norm = np.linalg.norm(B, axis=0) + B_max = np.abs(B).max(axis=0) # Same scale on roll and pitch B_norm[0] = max(B_norm[0], B_norm[1]) / np.sqrt(B.shape[0] / 2.0) @@ -193,7 +209,7 @@ def normalize_mix_px4(B): B_norm[5] = B_max[5] # Normalize - B_norm[np.abs(B_norm)<1e-3] = 1 + B_norm[np.abs(B_norm) < 1e-3] = 1 B_px = (B / B_norm) return B_px @@ -287,9 +303,9 @@ if __name__ == '__main__': parser.add_argument('-o', dest='outputfile', help='output header file') parser.add_argument('--normalize', help='Use normalized mixers (compatibility mode)', - action='store_true') + action='store_true') parser.add_argument('--sixdof', help='Use 6dof mixers', - action='store_true') + action='store_true') args = parser.parse_args() # Find toml files @@ -331,8 +347,8 @@ if __name__ == '__main__': # Generate header file header = generate_mixer_multirotor_header(geom_list, - use_normalized_mix=args.normalize, - use_6dof=args.sixdof) + use_normalized_mix=args.normalize, + use_6dof=args.sixdof) # print(header) # Write header file