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)