Fix some bugs in the python script for generating urdf files

Python/stairs/generate_stairs_urdf.py

@@ -10,15 +10,31 @@
 import xml.etree.ElementTree as et
 from xml.dom import minidom
 
-"""
-Step contain the definition of a step on a stair
-"""
+
 class Step:
+ """Step contain the definition of a step on a stair.
+
+ Attributes:
+ name (str): associated to the step.
+ length (double): horizontal dimension, in meters, of the step (along the x axis).
+ width (double): horizontal dimension, in meters, of the step (along the y axis).
+ height (double): vertical dimension, in meters, of the step (along the z axis).
+ """
  def __init__(self, step_index, size):
+ """Constructor of the step method
+ Args:
+ step_index (int): index representing the step.
+ size (list): 3d vector containing the length (x-dimension)
+ width (y-dimension) and height (z-dimension) of the step.
+ """
  self.name = 's_' + str(step_index)
- self.height = size[0]
- self.length = size[1]
- self.width = size[2]
+
+ if ((len(size) != 3) and all(element > 0 for element in size)):
+ raise ValueError('The size has to be a 3d-vector containing only positive numbers.')
+
+ self.length = size[0]
+ self.width = size[1]
+ self.height = size[2]
  self.mass = 1
 
  def append_urdf(self, model_urdf):
@@ -34,8 +50,8 @@ def append_urdf(self, model_urdf):
  iyy = str(self.mass/3.0 * (self.length ** 2 + self.height ** 2)),
  iyz= '0.0',
  izz=str(self.mass/3.0 * (self.length ** 2 + self.width ** 2)))
- 
- 
+
+
  visual = et.SubElement(link, 'visual')
  visual_origin = et.SubElement(visual, 'origin',
  xyz = str(self.length / 2.0) + ' 0.0 ' + str(self.height / 2.0),
@@ -44,7 +60,7 @@ def append_urdf(self, model_urdf):
  visual_geometry = et.SubElement(visual, 'geometry')
  visual_box = et.SubElement(visual_geometry, 'box',
  size = str(self.length) + ' ' + str(self.width) + ' ' + str(self.height))
- 
+
  collision = et.SubElement(link, 'collision')
  collision_origin = et.SubElement(collision, 'origin',
  xyz = str(self.length / 2.0) + ' 0.0 ' + str(self.height / 2.0),
@@ -53,21 +69,21 @@ def append_urdf(self, model_urdf):
  collision_geometry = et.SubElement(collision, 'geometry')
  collision_box = et.SubElement(collision_geometry, 'box',
  size = str(self.length) + ' ' + str(self.width) + ' ' + str(self.height))
- 
+
 def add_joint_to_urdf(parent_step, child_step, model_urdf):
  joint_name = 'joint_' + parent_step.name + '_' + child_step.name
- 
+
  joint = et.SubElement(model_urdf, 'joint', name = joint_name, type = 'fixed')
  et.SubElement(joint, 'origin',
  xyz = str(parent_step.length) + ' 0 ' + str(parent_step.height), rpy = '0 0 0')
  et.SubElement(joint, 'parent' ,link = parent_step.name)
  et.SubElement(joint, 'child' ,link = child_step.name)
- 
+
  return model_urdf
- 
+
 
 def save_urdf(model_urdf, output_file):
- 
+
  xmlstr = minidom.parseString(et.tostring(model_urdf, encoding='UTF-8')).toprettyxml(indent=' ')
  with open(output_file, 'w') as f:
  f.write(xmlstr)
@@ -83,7 +99,7 @@ def generate_urdf(model_name, step_size, steps_number):
  # Populate the urdf model with the steps
  for step in steps:
  step.append_urdf(model)
- 
+
  for i in range(len(steps) - 1):
  add_joint_to_urdf(steps[i], steps[i+1], model)
 
@@ -105,17 +121,17 @@ def main():
  parser.add_argument('--install_folder', type = str, default = './', required = False, help='File output')
  args = parser.parse_args()
 
- model_name = 'stair'
+ model_name = 'stairs'
  install_folder = args.install_folder
- 
- 
+
+
  model = generate_urdf(model_name,
  [args.step_length, args.step_width, args.step_height],
  args.steps_number)
 
  if not os.path.exists(os.path.normpath(args.install_folder)):
  os.makedirs(os.path.normpath(args.install_folder))
- 
+
  save_urdf(model,
  os.path.normpath(args.install_folder + '/' + model_name + '.urdf'))
 

cmake/GenerateAndAddUrdfModel.cmake

@@ -5,7 +5,7 @@
 find_package(PythonInterp 3.6 QUIET)
 checkandset_dependency(PythonInterp)
 
-find_package(PythonLibs  3.6 QUIET)
+find_package(PythonLibs 3.6 QUIET)
 checkandset_dependency(PythonLibs)
 
 framework_dependent_option(FRAMEWORK_GENERATE_urdf_models
@@ -41,8 +41,8 @@ function(generate_and_add_urdf_model)
 
  file(GLOB files "${CMAKE_BINARY_DIR}/Autogenerated/gazebo/models/${installation_folder}/*.urdf")
  install(FILES ${files}
- DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/share/gazebo/models/${installation_folder}")
+ DESTINATION "${CMAKE_INSTALL_PREFIX}/share/gazebo/models/${installation_folder}")
  install(FILES ${extra_files}
- DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/share/gazebo/models/${installation_folder}")
+ DESTINATION "${CMAKE_INSTALL_PREFIX}/share/gazebo/models/${installation_folder}")
 
 endfunction()