def test_cube_multiple_contacts(gazebo: scenario.GazeboSimulator,
get_model_str: Callable):
assert gazebo.initialize()
world = gazebo.get_world().to_gazebo()
# Insert the Physics system
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
# Insert the ground plane
assert world.insert_model(gym_ignition_models.get_model_file("ground_plane"))
assert len(world.model_names()) == 1
# Insert two cubes side to side with a 10cm gap
cube_urdf = misc.string_to_file(get_model_str())
assert world.insert_model(cube_urdf,
core.Pose([0, -0.15, 0.101], [1., 0, 0, 0]),
"cube1")
assert world.insert_model(cube_urdf,
core.Pose([0, 0.15, 0.101], [1., 0, 0, 0]),
"cube2")
assert len(world.model_names()) == 3
# Get the cubes
cube1 = world.get_model("cube1")
cube2 = world.get_model("cube2")
# Enable contact detection
assert not cube1.contacts_enabled()
assert cube1.enable_contacts(enable=True)
assert cube1.contacts_enabled()
# Enable contact detection
assert not cube2.contacts_enabled()
assert cube2.enable_contacts(enable=True)
assert cube2.contacts_enabled()
# The cubes were inserted floating in the air with a 1mm gap wrt to ground.
# There should be no contacts.
gazebo.run(paused=True)
assert not cube1.get_link("cube").in_contact()
assert not cube2.get_link("cube").in_contact()
assert len(cube1.contacts()) == 0
assert len(cube2.contacts()) == 0
# Make the cubes fall for 50ms
for _ in range(50):
gazebo.run()
assert cube1.get_link("cube").in_contact()
assert cube2.get_link("cube").in_contact()
assert len(cube1.contacts()) == 1
assert len(cube2.contacts()) == 1
# Now we make another cube fall above the gap. It will touch both cubes.
assert world.insert_model(cube_urdf,
core.Pose([0, 0, 0.301], [1., 0, 0, 0]),
"cube3")
assert len(world.model_names()) == 4
cube3 = world.get_model("cube3")
assert not cube3.contacts_enabled()
assert cube3.enable_contacts(enable=True)
assert cube3.contacts_enabled()
gazebo.run(paused=True)
assert not cube3.get_link("cube").in_contact()
assert len(cube3.contacts()) == 0
# Make the cube fall for 50ms
for _ in range(50):
gazebo.run()
# There will be two contacts, respectively with cube1 and cube2.
# Contacts are related to the link, not the collision elements. In the case of two
# collisions elements associated to the same link, contacts are merged together.
assert cube3.get_link("cube").in_contact()
assert len(cube3.contacts()) == 2
contact1 = cube3.contacts()[0]
contact2 = cube3.contacts()[1]
assert contact1.body_a == "cube3::cube"
assert contact2.body_a == "cube3::cube"
assert contact1.body_b == "cube1::cube"
assert contact2.body_b == "cube2::cube"
# Calculate the total contact wrench of cube3
assert cube3.get_link("cube").contact_wrench() == pytest.approx([0, 0, 50, 0, 0, 0],
abs=1.1)
# Calculate the total contact force of the cubes below.
# They will have 1.5 their weight from below and -0.5 from above.
assert cube1.get_link("cube").contact_wrench()[2] == pytest.approx(50, abs=1.1)
assert cube1.get_link("cube").contact_wrench()[2] == pytest.approx(50, abs=1.1)
# Check the normals and the sign of the forces
for contact in cube2.contacts():
if contact.body_b == "cube3::cube":
for point in contact.points:
assert point.force[2] < 0
assert point.normal == pytest.approx([0, 0, -1], abs=0.001)
if contact.body_b == "ground_plane::link":
for point in contact.points:
assert point.force[2] > 0
assert point.normal == pytest.approx([0, 0, 1], abs=0.001)
def __init__(self,
world: scenario.World,
name: str = 'ground',
position: List[float] = (0, 0, 0),
orientation: List[float] = (1, 0, 0, 0),
size: List[float] = (1.0, 1.0),
collision_thickness: float = 0.05,
friction: float = 5.0,
texture_dir: str = None,
np_random=None):
if np_random is None:
np_random = np.random.default_rng()
# Get a unique model name
model_name = get_unique_model_name(world, name)
# Initial pose
initial_pose = scenario.Pose(position, orientation)
# Find random PBR texture
albedo_map = None
normal_map = None
roughness_map = None
metalness_map = None
if texture_dir is not None:
# Get list of the available textures
textures = os.listdir(texture_dir)
# Keep only texture directories if texture_dir is a git repo (ugly fix)
try:
textures.remove('.git')
except:
pass
try:
textures.remove('README.md')
except:
pass
# Choose a random texture from these
random_texture_dir = os.path.join(texture_dir,
np_random.choice(textures))
# List all files
texture_files = os.listdir(random_texture_dir)
# Extract the appropriate files
for texture in texture_files:
texture_lower = texture.lower()
if 'basecolor' in texture_lower or 'albedo' in texture_lower:
albedo_map = os.path.join(random_texture_dir, texture)
elif 'normal' in texture_lower:
normal_map = os.path.join(random_texture_dir, texture)
elif 'roughness' in texture_lower:
roughness_map = os.path.join(random_texture_dir, texture)
elif 'specular' in texture_lower or 'metalness' in texture_lower:
metalness_map = os.path.join(random_texture_dir, texture)
# Create SDF string for the model
sdf = \
f'''<sdf version="1.7">
<model name="{model_name}">
<static>true</static>
<link name="{model_name}_link">
<collision name="{model_name}_collision">
<pose>0 0 {-collision_thickness/2} 0 0 0</pose>
<geometry>
<box>
<size>{size[0]} {size[1]} {collision_thickness}</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>{friction}</mu>
<mu2>{friction}</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name="{model_name}_visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>{size[0]} {size[1]}</size>
</plane>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>1 1 1 1</specular>
<pbr>
<metal>
{"<albedo_map>%s</albedo_map>"
% albedo_map if albedo_map is not None else ""}
{"<normal_map>%s</normal_map>"
% normal_map if normal_map is not None else ""}
{"<roughness_map>%s</roughness_map>"
% roughness_map if roughness_map is not None else ""}
{"<metalness_map>%s</metalness_map>"
% metalness_map if metalness_map is not None else ""}
</metal>
</pbr>
</material>
</visual>
</link>
</model>
</sdf>'''
# Convert it into a file
sdf_file = misc.string_to_file(sdf)
# Insert the model
ok_model = world.to_gazebo().insert_model(sdf_file, initial_pose,
model_name)
if not ok_model:
raise RuntimeError('Failed to insert ' + model_name)
# Get the model
model = world.get_model(model_name)
# Initialize base class
model_wrapper.ModelWrapper.__init__(self, model=model)
def test_full_panda_randomization():
# Get the URDF model
urdf_model = gym_ignition_models.get_model_file("panda")
# Convert it to a SDF string
sdf_model_string = scenario.urdffile_to_sdfstring(urdf_model)
# Write the SDF string to a temp file
sdf_model = misc.string_to_file(sdf_model_string)
# Create the randomizer
randomizer = sdf.SDFRandomizer(sdf_model=sdf_model)
joint_dynamics = randomizer.find_xpath("*/joint/axis/dynamics")
assert len(joint_dynamics) > 0
# Add the friction and damping elements since they're missing in the model
for joint_dynamic in joint_dynamics:
if joint_dynamic.find("friction") is None:
etree.SubElement(joint_dynamic, "friction")
friction = joint_dynamic.find("friction")
friction.text = str(0)
if joint_dynamic.find("damping") is None:
etree.SubElement(joint_dynamic, "damping")
damping = joint_dynamic.find("damping")
damping.text = str(3)
randomization_config = {
"*/link/inertial/mass": {
# mass + U(-0.5, 0.5)
"method": Method.Additive,
"distribution": Distribution.Uniform,
"params": UniformParams(low=-0.5, high=0.5),
"ignore_zeros": True,
"force_positive": True,
},
"*/link/inertial/inertia/ixx": {
# inertia * N(1, 0.2)
"method": Method.Coefficient,
"distribution": Distribution.Gaussian,
"params": GaussianParams(mean=1.0, variance=0.2),
"ignore_zeros": True,
"force_positive": True,
},
"*/link/inertial/inertia/iyy": {
"method": Method.Coefficient,
"distribution": Distribution.Gaussian,
"params": GaussianParams(mean=1.0, variance=0.2),
"ignore_zeros": True,
"force_positive": True,
},
"*/link/inertial/inertia/izz": {
"method": Method.Coefficient,
"distribution": Distribution.Gaussian,
"params": GaussianParams(mean=1.0, variance=0.2),
"ignore_zeros": True,
"force_positive": True,
},
"*/joint/axis/dynamics/friction": {
# friction in [0, 5]
"method": Method.Absolute,
"distribution": Distribution.Uniform,
"params": UniformParams(low=0, high=5),
"ignore_zeros": False, # We initialized the value as 0
"force_positive": True,
},
"*/joint/axis/dynamics/damping": {
# damping (= 3.0) * [0.8, 1.2]
"method": Method.Coefficient,
"distribution": Distribution.Uniform,
"params": UniformParams(low=0.8, high=1.2),
"ignore_zeros": True,
"force_positive": True,
},
# TODO: */joint/axis/limit/effort
}
for xpath, config in randomization_config.items():
randomizer.new_randomization().at_xpath(xpath).method(
config["method"]).sampled_from(
config["distribution"], config["params"]).force_positive(
config["distribution"]).ignore_zeros(
config["ignore_zeros"]).add()
randomizer.process_data()
assert len(randomizer.get_active_randomizations()) > 0
randomizer.sample(pretty_print=True)
def randomize_model_description(self, task: SupportedTasks,
**kwargs) -> str:
randomizer = self._get_sdf_randomizer(task=task)
sdf = misc.string_to_file(randomizer.sample())
return sdf
def __init__(self,
world: scenario.World,
name: Union[str, None] = None,
position: List[float] = (0, 0, 0),
orientation: List[float] = (1, 0, 0, 0),
camera_type: str = 'rgbd_camera',
width: int = 212,
height: int = 120,
update_rate: int = 15,
horizontal_fov: float = 1.567821,
vertical_fov: float = 1.022238,
clip_color: List[float] = (0.01, 1000.0),
clip_depth: List[float] = (0.01, 10.0),
noise_mean: float = None,
noise_stddev: float = None,
ros2_bridge_color: bool = False,
ros2_bridge_depth: bool = False,
ros2_bridge_points: bool = False,
visibility_mask: int = 2):
# Get a unique model name
if name is not None:
model_name = get_unique_model_name(world, name)
else:
model_name = get_unique_model_name(world, camera_type)
self._model_name = model_name
# Initial pose
initial_pose = scenario.Pose(position, orientation)
# Create SDF string for the model
sdf = \
f'''<sdf version="1.7">
<model name="{model_name}">
<static>true</static>
<link name="{model_name}_link">
<sensor name="camera" type="{camera_type}">
<topic>{model_name}</topic>
<always_on>1</always_on>
<update_rate>{update_rate}</update_rate>
<camera name="{model_name}_camera">
<image>
<width>{width}</width>
<height>{height}</height>
</image>
<horizontal_fov>{horizontal_fov}</horizontal_fov>
<vertical_fov>{vertical_fov}</vertical_fov>
<clip>
<near>{clip_color[0]}</near>
<far>{clip_color[1]}</far>
</clip>
{
f"""<depth_camera>
<clip>
<near>{clip_depth[0]}</near>
<far>{clip_depth[1]}</far>
</clip>
</depth_camera>""" if self.is_rgbd() else ""
}
{
f"""<noise>
<type>gaussian</type>
<mean>{noise_mean}</mean>
<stddev>{noise_stddev}</stddev>
</noise>""" if noise_mean is not None and noise_stddev is not None else ""
}
<visibility_mask>{visibility_mask}</visibility_mask>
</camera>
</sensor>
</link>
</model>
</sdf>'''
# Convert it into a file
sdf_file = misc.string_to_file(sdf)
# Insert the model
ok_model = world.to_gazebo().insert_model(sdf_file, initial_pose,
model_name)
if not ok_model:
raise RuntimeError('Failed to insert ' + model_name)
# Get the model
model = world.get_model(model_name)
# Initialize base class
model_wrapper.ModelWrapper.__init__(self, model=model)
if ros2_bridge_color or ros2_bridge_depth or ros2_bridge_points:
from threading import Thread
# Note: This unfortunately hinders use of SIGINT
threads = []
if ros2_bridge_color:
thread = Thread(target=self.construct_ros2_bridge,
args=(self.color_topic(),
'sensor_msgs/msg/Image',
'ignition.msgs.Image'),
daemon=True)
threads.append(thread)
if ros2_bridge_depth:
thread = Thread(target=self.construct_ros2_bridge,
args=(self.depth_topic(),
'sensor_msgs/msg/Image',
'ignition.msgs.Image'),
daemon=True)
threads.append(thread)
if ros2_bridge_points:
thread = Thread(target=self.construct_ros2_bridge,
args=(self.points_topic(),
'sensor_msgs/msg/PointCloud2',
'ignition.msgs.PointCloudPacked'),
daemon=True)
threads.append(thread)
for thread in threads:
thread.start()
def __init__(self,
world: scenario.World,
name: str = 'ground',
position: List[float] = (0, 0, 0),
orientation: List[float] = (1, 0, 0, 0),
size: List[float] = (1.0, 1.0),
collision_thickness=0.05,
friction: float = 5.0):
# Get a unique model name
model_name = get_unique_model_name(world, name)
# Initial pose
initial_pose = scenario.Pose(position, orientation)
# Create SDF string for the model
sdf = \
f'''<sdf version="1.7">
<model name="{model_name}">
<static>true</static>
<link name="{model_name}_link">
<collision name="{model_name}_collision">
<pose>0 0 {-collision_thickness/2} 0 0 0</pose>
<geometry>
<box>
<size>{size[0]} {size[1]} {collision_thickness}</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>{friction}</mu>
<mu2>{friction}</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name="{model_name}_visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>{size[0]} {size[1]}</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
</sdf>'''
# Convert it into a file
sdf_file = misc.string_to_file(sdf)
# Insert the model
ok_model = world.to_gazebo().insert_model(sdf_file, initial_pose,
model_name)
if not ok_model:
raise RuntimeError('Failed to insert ' + model_name)
# Get the model
model = world.get_model(model_name)
# Initialize base class
model_wrapper.ModelWrapper.__init__(self, model=model)
