示例#1
0
    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)
示例#2
0
    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)
示例#3
0
    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)
示例#4
0
    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)
示例#6
0
    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"
示例#7
0
    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)
示例#8
0
    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)
示例#10
0
    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()