def __init__(
self,
world: scenario.World,
position: List[float] = (0.0, 0.0, 0.0),
orientation: List[float] = (1.0, 0, 0, 0),
model_file: str = None,
):
# Get a unique model name
model_name = get_unique_model_name(world, "pendulum")
# Initial pose
initial_pose = scenario.Pose(position, orientation)
# Get the model file (URDF or SDF) and allow to override it
if model_file is None:
model_file = Pendulum.get_model_file()
# Insert the model
ok_model = world.to_gazebo().insert_model(model_file, initial_pose, model_name)
if not ok_model:
raise RuntimeError("Failed to insert model")
# Get the model
model = world.get_model(model_name)
# Initialize base class
super().__init__(model=model)
def __init__(self,
world: scenario.World,
name: str = 'ur5_rg2',
position: List[float] = (0, 0, 0),
orientation: List[float] = (1, 0, 0, 0),
model_file: str = None,
use_fuel: bool = True,
arm_collision: bool = True,
hand_collision: bool = True,
separate_gripper_controller: bool = True,
initial_joint_positions: List[float] = (0.0, 0.0, 1.57, 0.0,
-1.57, -1.57, 0.0,
0.0)):
# Get a unique model name
model_name = get_unique_model_name(world, name)
# Initial pose
initial_pose = scenario.Pose(position, orientation)
# Get the default model description (URDF or SDF) allowing to pass a custom model
if model_file is None:
model_file = self.get_model_file(fuel=use_fuel)
if not arm_collision or not hand_collision:
model_file = self.disable_collision(model_file=model_file,
arm_collision=arm_collision,
hand_collision=hand_collision)
# Insert the model
ok_model = world.to_gazebo().insert_model(model_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)
self.__separate_gripper_controller = separate_gripper_controller
# Set initial joint configuration
self.__set_initial_joint_positions(initial_joint_positions)
if not model.to_gazebo().reset_joint_positions(
self.get_initial_joint_positions(), self.get_joint_names()):
raise RuntimeError("Failed to set initial robot joint positions")
# Add JointStatePublisher to UR5 + RG2
self.__add_joint_state_publisher(model)
# Add JointTrajectoryController to UR5 + RG2
self.__add_joint_trajectory_controller(model)
# Initialize base class
super().__init__(model=model)
def __init__(self,
world: scenario.World,
name: str = 'cylinder',
position: List[float] = (0, 0, 0),
orientation: List[float] = (1, 0, 0, 0),
radius: float = 0.025,
length: float = 0.1,
mass: float = 0.1,
static: bool = False,
collision: bool = True,
friction: float = 1.0,
visual: bool = True,
gui_only: bool = False,
color: List[float] = (0.8, 0.8, 0.8, 1.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 = self.get_sdf(model_name=model_name,
radius=radius,
length=length,
mass=mass,
static=static,
collision=collision,
friction=friction,
visual=visual,
gui_only=gui_only,
color=color)
# 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 __init__(self,
world: scenario.World,
name: str = 'primitive',
use_specific_primitive: Union[str, None] = None,
position: List[float] = (0, 0, 0),
orientation: List[float] = (1, 0, 0, 0),
static: bool = False,
collision: bool = True,
visual: bool = True,
gui_only: bool = False,
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)
# Create SDF string for the model
sdf = self.get_sdf(model_name=model_name,
use_specific_primitive=use_specific_primitive,
static=static,
collision=collision,
visual=visual,
gui_only=gui_only,
np_random=np_random)
# 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 __init__(self,
world: scenario.World,
name: str = 'object',
position: List[float] = (0, 0, 0),
orientation: List[float] = (1, 0, 0, 0),
model_paths: str = None,
owner: str = 'GoogleResearch',
collection: str = 'Google Scanned Objects',
server: str = 'https://fuel.ignitionrobotics.org',
server_version: str = '1.0',
unique_cache: bool = False,
reset_collection: bool = False,
np_random=None):
# Get a unique model name
model_name = get_unique_model_name(world, name)
# Initial pose
initial_pose = scenario.Pose(position, orientation)
model_collection_randomizer = ModelCollectionRandomizer(
model_paths=model_paths,
owner=owner,
collection=collection,
server=server,
server_version=server_version,
unique_cache=unique_cache,
reset_collection=reset_collection,
np_random=np_random)
# Note: using default arguments here
modified_sdf_file = model_collection_randomizer.random_model()
# Insert the model
ok_model = world.to_gazebo().insert_model(modified_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 __init__(
self,
world: scenario_core.World,
position: List[float],
orientation: List[float],
model_file: str = None,
):
# Get a unique model name
model_name = get_unique_model_name(world, "icub")
# Initial pose
initial_pose = scenario_core.Pose(position, orientation)
# Get the model file (URDF or SDF) and allow to override it
if model_file is None:
model_file = self.get_model_file()
# Insert the model
ok_model = world.to_gazebo().insert_model(model_file, initial_pose,
model_name)
if not ok_model:
raise RuntimeError("Failed to insert model")
# Get the model
model = world.get_model(model_name)
# Initialize base class
super().__init__(model=model)
# Store the initial positions
q0 = list(self.initial_positions.values())
joint_names = list(self.initial_positions.keys())
assert self.dofs() == len(q0) == len(joint_names)
ok_q0 = self.to_gazebo().reset_joint_positions(q0, joint_names)
assert ok_q0, "Failed to set initial position"
def __init__(self,
world: scenario.World,
name: str = 'plane',
position: List[float] = (0, 0, 0),
orientation: List[float] = (1, 0, 0, 0),
size: List[float] = (1.0, 1.0),
direction: List[float] = (0.0, 0.0, 1.0),
collision: bool = True,
friction: float = 1.0,
visual: bool = True):
# 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">
{
f"""
<collision name="{model_name}_collision">
<geometry>
<plane>
<normal>{direction[0]} {direction[1]} {direction[2]}</normal>
<size>{size[0]} {size[1]}</size>
</plane>
</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>
""" if collision else ""
}
{
f"""
<visual name="{model_name}_visual">
<geometry>
<plane>
<normal>{direction[0]} {direction[1]} {direction[2]}</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>
""" if visual else ""
}
</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 __init__(self,
world: scenario.World,
position: List[float] = (0.0, 0.0, 0.0),
orientation: List[float] = (1.0, 0, 0, 0),
model_file: str = None):
# Get a unique model name
model_name = get_unique_model_name(world, "panda")
# Initial pose
initial_pose = scenario.Pose(position, orientation)
# Get the default model description (URDF or SDF) allowing to pass a custom model
if model_file is None:
model_file = Panda.get_model_file()
# Insert the model
ok_model = world.to_gazebo().insert_model(model_file,
initial_pose,
model_name)
if not ok_model:
raise RuntimeError("Failed to insert model")
# Get the model
model = world.get_model(model_name)
# Initial joint configuration
model.to_gazebo().reset_joint_positions(
[0, -0.785,0, -2.356, 0, 1.571, 0.785],
[name for name in model.joint_names() if "panda_joint" in name])
# From:
# https://github.com/mkrizmancic/franka_gazebo/blob/master/config/default.yaml
pid_gains_1000hz = {
'panda_joint1': scenario.PID(50, 0, 20),
'panda_joint2': scenario.PID(10000, 0, 500),
'panda_joint3': scenario.PID(100, 0, 10),
'panda_joint4': scenario.PID(1000, 0, 50),
'panda_joint5': scenario.PID(100, 0, 10),
'panda_joint6': scenario.PID(100, 0, 10),
'panda_joint7': scenario.PID(10, 0.5, 0.1),
'panda_finger_joint1': scenario.PID(100, 0, 50),
'panda_finger_joint2': scenario.PID(100, 0, 50),
}
# Check that all joints have gains
if not set(model.joint_names()) == set(pid_gains_1000hz.keys()):
raise ValueError("The number of PIDs does not match the number of joints")
# Set the PID gains
for joint_name, pid in pid_gains_1000hz.items():
if not model.get_joint(joint_name).set_pid(pid=pid):
raise RuntimeError(f"Failed to set the PID of joint '{joint_name}'")
# Set the default PID update period
assert model.set_controller_period(1000.0)
# Initialize base class
super().__init__(model=model)
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 __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()
