def test_xml_input_random(self): sdf_elements = [ 'mass', 'child', 'parent', 'pose', 'box', 'cylinder', 'sphere', 'mesh', 'limit', 'inertial', 'inertia' ] for sdf_name in sdf_elements: obj = create_sdf_element(sdf_name) assert obj is not None obj.random() output = subprocess.check_output( ['pcg-sdf2urdf', '--xml', obj.to_xml_as_str(), '--print']) urdf = parse_urdf(output.decode('utf-8')) assert urdf is not None response_sdf = urdf2sdf(urdf) assert obj == response_sdf # Test for dynamics obj = create_sdf_element('dynamics') assert obj is not None obj.reset(with_optional_elements=True) obj.damping.random() obj.friction.random() output = subprocess.check_output( ['pcg-sdf2urdf', '--xml', obj.to_xml_as_str(), '--print']) urdf = parse_urdf(output.decode('utf-8')) assert urdf is not None response_sdf = urdf2sdf(urdf) assert obj.damping == response_sdf.damping assert obj.friction == response_sdf.friction
def add_collision(self, mesh, link, model_path, fraction_of_visual=0.05, min_faces=8, max_faces=750, friction=1.0): # Determine name of path to the collistion geometry collision_name = link.attributes['name'] + \ '_collision_' + str(len(link.collisions)) collision_mesh_path = self.get_collision_mesh_path( model_path, collision_name) # Determine number of faces to keep after the decimation face_count = \ min( max( fraction_of_visual*len(mesh.faces), min_faces ), max_faces ) # Simplify mesh via decimation collision_mesh = mesh.simplify_quadratic_decimation(face_count) # Export the collision mesh to the appropriate location os.makedirs(os.path.dirname(collision_mesh_path), exist_ok=True) collision_mesh.export(collision_mesh_path, file_type=self.__collision_mesh_file_type) # Create collision SDF element collision = create_sdf_element('collision') # Add collision geometry to the SDF collision.geometry.mesh = create_sdf_element('mesh') collision.geometry.mesh.uri = os.path.relpath(collision_mesh_path, start=model_path) # Add surface friction to the SDF of collision (default to 1 and randomize later) collision.surface = create_sdf_element('surface') collision.surface.friction = create_sdf_element('friction', 'surface') collision.surface.friction.ode = create_sdf_element('ode', 'collision') collision.surface.friction.ode.mu = friction collision.surface.friction.ode.mu2 = friction # Add it to the SDF of the link collision_name = os.path.basename(collision_mesh_path).split('.')[0] link.add_collision(collision_name, collision)
def test_load_from_sdf(self): lights = [Light(name=generate_random_string(5)) for _ in range(3)] models = [ SimulationModel(name=generate_random_string(5)) for _ in range(2) ] # Test import from a list of SDF elements sdf_elements = [light.to_sdf() for light in lights] + \ [model.to_sdf() for model in models] group = ModelGroup.from_sdf(sdf_elements) self.assertIsNotNone(group) self.assertEqual(group.n_models, len(models)) self.assertEqual(group.n_lights, len(lights)) # Test import from one single SDF element sdf = create_sdf_element('sdf') for model in models: sdf.add_model(model.name, model.to_sdf()) for light in lights: sdf.add_light(light.name, light.to_sdf()) group = ModelGroup.from_sdf(sdf) self.assertIsNotNone(group) self.assertEqual(group.n_models, len(models)) self.assertEqual(group.n_lights, len(lights))
def generate_string_test_obj(name, value=None): if value is None: value = generate_random_string(5) sdf_str = '<{}>{}</{}>'.format(name, value, name) expected_sdf = create_sdf_element(name) self.assertIsNotNone(expected_sdf, '{} returned None'.format(name)) expected_sdf.value = value return sdf_str, expected_sdf
def generate_boolean_test_obj(name, value=None): if value is None: value = random.choice([True, False]) sdf_str = '<{}>{}</{}>'.format(name, int(value), name) expected_sdf = create_sdf_element(name) self.assertIsNotNone(expected_sdf, '{} returned None'.format(name)) expected_sdf.value = value return sdf_str, expected_sdf
def generate_array_test_obj(name, vec=None): expected_sdf = create_sdf_element(name) self.assertIsNotNone(expected_sdf, '{} returned None'.format(name)) if vec is None: vec = [random.random() for _ in range(expected_sdf._size)] sdf_str = '<{}>{}</{}>'.format(name, ' '.join(str(x) for x in vec), name) expected_sdf.value = vec return sdf_str, expected_sdf
def test_random_sdf(self): sdf_elements = [ 'mass', 'child', 'parent', 'pose', 'box', 'cylinder', 'sphere', 'mesh', 'limit', 'inertial', 'inertia', 'joint', 'link', 'model', 'visual', 'collision', 'image' ] for sdf_name in sdf_elements: obj = create_sdf_element(sdf_name) obj.random() subprocess.check_output( ['pcg-sdflint', '--xml', obj.to_xml_as_str()])
def test_xml_input_visual_collision(self): for sdf_tag in ['visual', 'collision']: obj = create_sdf_element(sdf_tag) assert obj is not None obj.pose = create_sdf_element('pose') obj.pose.random() obj.geometry = create_sdf_element('geometry') geometries = ['box', 'cylinder', 'sphere', 'mesh'] for geo_name in geometries: obj.geometry.reset(mode=geo_name) getattr(obj.geometry, geo_name).random() output = subprocess.check_output( ['pcg-sdf2urdf', '--xml', obj.to_xml_as_str(), '--print']) urdf = parse_urdf(output.decode('utf-8')) assert urdf is not None response_sdf = urdf2sdf(urdf) assert obj == response_sdf
def main(): # Total mass taken from datasheet (given as ~18.0kg) # You can also use your own estimate of total mass if you managed to weigh Panda yourself :) total_mass = 18.0 if len(sys.argv) > 1: if float(sys.argv[1]) > 0.0: total_mass = float(sys.argv[1]) else: print( "Error: Total mass of Panda (first argument) must be positive." ) exit(1) print('Estimating inertial properties for each link to add up to %f kg' % total_mass) # Percentage of mass to redistribute from hand to fingers due to internal mechanical coupling # Choose whatever feels right (default value here is guesstimated) pct_mass_of_hand = 0.75 if len(sys.argv) > 2: if float(sys.argv[2]) >= 0.0 and float(sys.argv[2]) < 1.0: pct_mass_of_hand = float(sys.argv[2]) else: print( "Error: Percentage of hand's mass to redistribute (second argument) must be in range [0.0, 1.0)." ) exit(1) # Get path to all visual meshes visual_mesh_dir = path.join( path.dirname(path.dirname(path.realpath(__file__))), 'panda', 'meshes', 'visual') visual_mesh_basenames = listdir(visual_mesh_dir) visual_mesh_basenames.sort() # Load all meshes meshes = {} for mesh_basename in visual_mesh_basenames: link_name = path.splitext(mesh_basename)[0] mesh_path = path.join(visual_mesh_dir, mesh_basename) meshes[link_name] = trimesh.load(mesh_path, force='mesh', ignore_materials=True) # Compute the total volume of the robot in order to estimate the required density total_volume = 0.0 for link_name in meshes: mesh = meshes[link_name] print('Volume estimate of %s: %f m^3' % (link_name, mesh.volume)) if link_name == 'finger': total_volume += 2 * mesh.volume print('Note: Finger volume added twice to the total volume') else: total_volume += mesh.volume # Compute average density average_density = total_mass / total_volume print('Average density estimate: %f kg/m^3' % average_density) # Estimate inertial properties for each link mass = {} inertia = {} centre_of_mass = {} for link_name in meshes: mesh = meshes[link_name] mesh.density = average_density mass[link_name] = mesh.mass inertia[link_name] = mesh.moment_inertia centre_of_mass[link_name] = mesh.center_mass # Redistribute X% of the hand's mass to the fingers due to internal mechanical coupling # This improves reliability of grasps and makes fingers less susceptible to disturbances print("Redistributing %f%% of hand's mass fingers" % (100 * pct_mass_of_hand)) old_mass_finger = mass['finger'] # Add half of hand's redistributed mass to each finger finger_extra_mass = (pct_mass_of_hand / 2) * mass['hand'] mass['finger'] += finger_extra_mass # Then, update inertial proportionally to mass increase ratio inertia['finger'] *= mass['finger'] / old_mass_finger # Recompute centre of finger's mass to account for this redistribution # TODO: Read translation directly from SDF (currently copied and hard-coded) translation_hand_finger = [0.0, 0.0, 0.0584] for i in range(3): centre_of_mass['finger'][i] = ( old_mass_finger * centre_of_mass['finger'][i] + finger_extra_mass * (centre_of_mass['hand'][i] - translation_hand_finger[i]) ) / mass['finger'] # Reduce mass and inertia of hand to (1.0-X)% in order to account for redistribution of its mass mass['hand'] *= (1.0 - pct_mass_of_hand) inertia['hand'] *= (1.0 - pct_mass_of_hand) # Create a new SDF with one model sdf = SDF() sdf.add_model(name='panda') model = sdf.models[0] # Set inertial properties for each link into the SDF for link_name in meshes: link = create_sdf_element('link') link.mass = mass[link_name] link.inertia.ixx = inertia[link_name][0][0] link.inertia.iyy = inertia[link_name][1][1] link.inertia.izz = inertia[link_name][2][2] link.inertia.ixy = inertia[link_name][0][1] link.inertia.ixz = inertia[link_name][0][2] link.inertia.iyz = inertia[link_name][1][2] link.inertial.pose = [ centre_of_mass[link_name][0], centre_of_mass[link_name][1], centre_of_mass[link_name][2], 0.0, 0.0, 0.0 ] model.add_link(link_name, link) # Write into output file output_file = 'panda_inertial_out.sdf' sdf.export_xml(output_file) print('Results written into "%s"' % output_file)
def main(): # Total mass taken from datasheet (given as 18.4kg and 0.78kg respectively for UR5 and RG2) ur5_mass = 18.4 rg2_mass = 0.78 print( f'Estimating inertial properties for each link to add up to {ur5_mass}kg for UR5 and {rg2_mass}kg for RG2') # Percentage of mass to redistribute from hand to fingers due to internal mechanical coupling # Choose whatever feels right (default value here is guesstimated) pct_mass_of_hand = 0.5 # Get path to all visual meshes ur5_visual_mesh_dir = path.join(path.dirname(path.dirname( path.realpath(__file__))), 'ur5_rg2', 'meshes', 'visual', 'ur5') ur5_visual_mesh_basenames = listdir(ur5_visual_mesh_dir) ur5_visual_mesh_basenames.sort() rg2_visual_mesh_dir = path.join(path.dirname(path.dirname( path.realpath(__file__))), 'ur5_rg2', 'meshes', 'visual', 'rg2') rg2_visual_mesh_basenames = listdir(rg2_visual_mesh_dir) rg2_visual_mesh_basenames.sort() # Load all meshes ur5_meshes = {} for mesh_basename in ur5_visual_mesh_basenames: ur5_link_name = path.splitext(mesh_basename)[0] ur5_mesh_path = path.join(ur5_visual_mesh_dir, mesh_basename) ur5_meshes[ur5_link_name] = trimesh.load(ur5_mesh_path, force='mesh', ignore_materials=True) rg2_meshes = {} for mesh_basename in rg2_visual_mesh_basenames: rg2_link_name = path.splitext(mesh_basename)[0] rg2_mesh_path = path.join(rg2_visual_mesh_dir, mesh_basename) rg2_meshes[rg2_link_name] = trimesh.load(rg2_mesh_path, force='mesh', ignore_materials=True) # Compute the total volume of the robot in order to estimate the required density ur5_total_volume = 0.0 for link_name in ur5_meshes: ur5_mesh = ur5_meshes[link_name] print('Volume estimate of %s: %f m^3' % (link_name, ur5_mesh.volume)) ur5_total_volume += ur5_mesh.volume rg2_total_volume = 0.0 for link_name in rg2_meshes: rg2_mesh = rg2_meshes[link_name] print('Volume estimate of %s: %f m^3' % (link_name, rg2_mesh.volume)) if link_name == 'finger': rg2_total_volume += 2*rg2_mesh.volume print('Note: Finger volume added twice to the total volume') else: rg2_total_volume += rg2_mesh.volume # Compute average density ur5_average_density = ur5_mass/ur5_total_volume print('Average density estimate for UR5: %f kg/m^3' % ur5_average_density) rg2_average_density = rg2_mass/rg2_total_volume print('Average density estimate for RG2: %f kg/m^3' % rg2_average_density) # Estimate inertial properties for each link ur5_mass = {} ur5_inertia = {} ur5_centre_of_mass = {} for link_name in ur5_meshes: ur5_mesh = ur5_meshes[link_name] ur5_mesh.density = ur5_average_density ur5_mass[link_name] = ur5_mesh.mass ur5_inertia[link_name] = ur5_mesh.moment_inertia ur5_centre_of_mass[link_name] = ur5_mesh.center_mass rg2_mass = {} rg2_inertia = {} rg2_centre_of_mass = {} for link_name in rg2_meshes: rg2_mesh = rg2_meshes[link_name] rg2_mesh.density = rg2_average_density rg2_mass[link_name] = rg2_mesh.mass rg2_inertia[link_name] = rg2_mesh.moment_inertia rg2_centre_of_mass[link_name] = rg2_mesh.center_mass # Redistribute X% of the hand's mass to the fingers due to internal mechanical coupling # This improves reliability of grasps and makes fingers less susceptible to disturbances print("Redistributing %f%% of hand's mass fingers" % (100*pct_mass_of_hand)) old_mass_finger = rg2_mass['finger'] # Add half of hand's redistributed mass to each finger finger_extra_mass = (pct_mass_of_hand/2)*rg2_mass['hand'] rg2_mass['finger'] += finger_extra_mass # Then, update inertial proportionally to mass increase ratio rg2_inertia['finger'] *= rg2_mass['finger']/old_mass_finger # Recompute centre of finger's mass to account for this redistribution # TODO: Read translation directly from SDF (currently copied and hard-coded) translation_hand_finger = [0.105, 0.017, 0.0] for i in range(3): rg2_centre_of_mass['finger'][i] = (old_mass_finger * rg2_centre_of_mass['finger'][i] + finger_extra_mass * (rg2_centre_of_mass['hand'][i]-translation_hand_finger[i])) / rg2_mass['finger'] # Reduce mass and inertia of hand to (1.0-X)% in order to account for redistribution of its mass rg2_mass['hand'] *= (1.0 - pct_mass_of_hand) rg2_inertia['hand'] *= (1.0 - pct_mass_of_hand) # Create a new SDF with one model sdf = SDF() sdf.add_model(name='UR5') ur5_model = sdf.models[0] sdf.add_model(name='RG2') rg2_model = sdf.models[1] # Set inertial properties for each link into the SDF for link_name in ur5_meshes: link = create_sdf_element('link') link.mass = ur5_mass[link_name] link.inertia.ixx = ur5_inertia[link_name][0][0] link.inertia.iyy = ur5_inertia[link_name][1][1] link.inertia.izz = ur5_inertia[link_name][2][2] link.inertia.ixy = ur5_inertia[link_name][0][1] link.inertia.ixz = ur5_inertia[link_name][0][2] link.inertia.iyz = ur5_inertia[link_name][1][2] link.inertial.pose = [ur5_centre_of_mass[link_name][0], ur5_centre_of_mass[link_name][1], ur5_centre_of_mass[link_name][2], 0.0, 0.0, 0.0] ur5_model.add_link(link_name, link) for link_name in rg2_meshes: link = create_sdf_element('link') link.mass = rg2_mass[link_name] link.inertia.ixx = rg2_inertia[link_name][0][0] link.inertia.iyy = rg2_inertia[link_name][1][1] link.inertia.izz = rg2_inertia[link_name][2][2] link.inertia.ixy = rg2_inertia[link_name][0][1] link.inertia.ixz = rg2_inertia[link_name][0][2] link.inertia.iyz = rg2_inertia[link_name][1][2] link.inertial.pose = [rg2_centre_of_mass[link_name][0], rg2_centre_of_mass[link_name][1], rg2_centre_of_mass[link_name][2], 0.0, 0.0, 0.0] rg2_model.add_link(link_name, link) # Write into output file output_file = 'ur5_rg2_inertial_out.sdf' sdf.export_xml(output_file) print('Results written into "%s"' % output_file)
# Import the element creator from pcg_gazebo.parsers.sdf import create_sdf_element # Material elements are initially empty since all its child elements are optional material = create_sdf_element('material') # To create all optional elements with its default elements, use reset() material.reset(with_optional_elements=True) print(material)