Ejemplo n.º 1
0
    def __init__(self,
                 model_file: str,
                 gazebo: bindings.GazeboWrapper,
                 controller_rate: float = None) -> None:

        # Find the model file
        model_abs_path = resource_finder.find_resource(model_file)

        # Initialize the parent classes
        robot_abc.RobotABC.__init__(self)
        robot_baseframe.RobotBaseFrame.__init__(self)
        robot_initialstate.RobotInitialState.__init__(self)
        self.model_file = model_abs_path

        # Private attributes
        self._gazebo = gazebo
        self._robot_name = None
        self._gympp_robot = None
        self._base_frame = None
        self._joint_names = None
        self._controller_rate = controller_rate

        # Create a random prefix that will be used for the robot name
        letters_and_digits = string.ascii_letters + string.digits
        self._prefix = ''.join(
            random.choice(letters_and_digits) for _ in range(6))
Ejemplo n.º 2
0
    def __init__(self,
                 p,
                 model_file: str,
                 plane_id: int = None,
                 keep_fixed_joints: bool = False) -> None:

        self._pybullet = p
        self._plane_id = plane_id
        self._keep_fixed_joints = keep_fixed_joints

        # Find the model file
        model_abs_path = resource_finder.find_resource(model_file)

        # Initialize the parent classes
        robot.robot_abc.RobotABC.__init__(self)
        robot.robot_baseframe.RobotBaseFrame.__init__(self)
        robot.robot_initialstate.RobotInitialState.__init__(self)
        self.model_file = model_abs_path

        # Private attributes
        self._robot_id = None
        self._links_name2index_dict = None
        self._joints_name2index_dict = None

        # TODO
        self._base_frame = None
        self._base_constraint = None

        # Create a map from the joint name to its control info
        self._jointname2jointcontrolinfo = dict()
Ejemplo n.º 3
0
    def get_model_loader(model_file: str, considered_joints: List[str] = None):

        # Find the urdf file
        urdf_file = resource_finder.find_resource(file_name=model_file)

        # Get the file extension
        folder, model_file = os.path.split(urdf_file)
        model_name, extension = os.path.splitext(model_file)

        if extension == ".sdf":
            raise RuntimeError(
                "SDF models are not currently supported by iDynTree")

        # Create the model loader
        mdl_loader = idt.ModelLoader()

        # Load the urdf model
        if considered_joints is None:
            ok_load = mdl_loader.loadModelFromFile(urdf_file)
        else:
            ok_load = mdl_loader.loadReducedModelFromFile(
                urdf_file, considered_joints)

        if not ok_load:
            raise RuntimeError(f"Failed to load model from file '{urdf_file}'")

        return mdl_loader
Ejemplo n.º 4
0
    def gympp_robot(self):
        if self._gympp_robot:
            assert not self._gympp_robot.expired(), "The Robot object has expired"
            assert self._gympp_robot.lock(), "The Robot object is empty"
            assert self._gympp_robot.lock().valid(), "The Robot object is not valid"
            return self._gympp_robot.lock()

        # Find and load the model SDF file
        sdf_file = resource_finder.find_resource(self.model_file)
        with open(sdf_file, "r") as stream:
            sdf_string = stream.read()

        # Get the model name
        original_name = bindings.GazeboWrapper.getModelNameFromSDF(sdf_string)
        assert original_name, f"Failed to get model name from file {self.model_file}"

        # Create a unique robot name
        self._robot_name = self._prefix + "::" + original_name

        # Initialize the model data
        model_data = bindings.ModelInitData()
        model_data.setModelName(self._robot_name)
        model_data.setSdfString(sdf_string)
        model_data.setPosition([0., 0, 0])  # TODO: default initial position
        model_data.setOrientation([1., 0, 0, 0])  # TODO: default initial orientation

        # Initialize robot controller plugin
        plugin_data = bindings.PluginData()
        plugin_data.setLibName("RobotController")
        plugin_data.setClassName("gympp::plugins::RobotController")

        # Insert the model
        ok_model = self._gazebo.insertModel(model_data, plugin_data)
        assert ok_model, "Failed to insert the model"

        # Extract the robot from the singleton
        self._gympp_robot = bindings.RobotSingleton_get().getRobot(self._robot_name)

        # The robot is a weak pointer. Check that it is valid.
        assert not self._gympp_robot.expired(), "The Robot object has expired"
        assert self._gympp_robot.lock(), \
            "The returned Robot object does not contain a valid interface"
        assert self._gympp_robot.lock().valid(), "The Robot object is not valid"

        if self._controller_rate:
            logger.debug("Robot controller rate: {} Hz".format(self._controller_rate))
            ok_dt = self._gympp_robot.lock().setdt(1 / self._controller_rate)
            assert ok_dt, "Failed to set the robot controller period"

        logger.debug(f"IgnitionRobot '{self._gympp_robot.lock().name()}' added to the "
                     "simulation")
        return self._gympp_robot.lock()
Ejemplo n.º 5
0
    def __init__(self,
                 physics_rate: float,
                 iterations: int = int(1),
                 rtf=float(np.finfo(np.float32).max)):
        self.simulator = bindings.GazeboWrapper(iterations, rtf, physics_rate)
        assert self.simulator

        self.simulator.setVerbosity(4)

        empty_world = resource_finder.find_resource("DefaultEmptyWorld.world")
        ok_world = self.simulator.setupGazeboWorld(worldFile=empty_world)
        assert ok_world

        ok_initialize = self.simulator.initialize()
        assert ok_initialize
Ejemplo n.º 6
0
    def pybullet(self) -> bullet_client.BulletClient:
        if self._pybullet is not None:
            return self._pybullet

        logger.debug("Creating PyBullet simulator")

        if self._render_enabled:
            self._pybullet = bullet_client.BulletClient(pybullet.GUI)
        else:
            # Connects to an existing instance or, if it fails, creates an headless
            # simulation (DIRECT)
            self._pybullet = bullet_client.BulletClient()

        assert self._pybullet, "Failed to create the bullet client"

        # Find the ground plane
        resource_finder.add_path(pybullet_data.getDataPath())
        world_abs_path = resource_finder.find_resource(self._world)

        # Load the ground plane
        self._plane_id = self._load_model(world_abs_path)

        # Configure the physics engine
        self._pybullet.setGravity(0, 0, -9.81)
        self._pybullet.setPhysicsEngineParameter(numSolverIterations=10)

        # Configure the physics engine with a single big time step divided in multiple
        # substeps. As an alternative, we could use a single substep and step the
        # simulation multiple times.
        self._pybullet.setTimeStep(1.0 / self._physics_rate /
                                   self._num_of_physics_steps)
        self._pybullet.setPhysicsEngineParameter(
            numSubSteps=self._num_of_physics_steps)

        # Disable real-time update. We step the simulation when needed.
        self._pybullet.setRealTimeSimulation(0)

        logger.info("PyBullet Physic Engine Parameters:")
        logger.info(str(self._pybullet.getPhysicsEngineParameters()))

        step_time = 1.0 / self._physics_rate / self._rtf
        logger.info(f"Nominal step time: {step_time} seconds")

        logger.debug("PyBullet simulator created")
        return self._pybullet
Ejemplo n.º 7
0
    def __init__(self,
                 p: pybullet,
                 model_file: str,
                 plane_id: int = None,
                 keep_fixed_joints: bool = False) -> None:

        self._pybullet = p
        self._plane_id = plane_id
        self._keep_fixed_joints = keep_fixed_joints

        # Find the model file
        model_abs_path = resource_finder.find_resource(model_file)

        # Initialize the parent classes
        super().__init__()
        self.model_file = model_abs_path

        # Load the model
        self._robot_id = self._load_model(self.model_file)
        assert self._robot_id is not None, "Failed to load the robot model"

        # Other private attributes
        self._links_name2index_dict = None
        self._joints_name2index_dict = None

        # TODO
        self._base_frame = None
        self._base_constraint = None
        self._initial_joint_positions = None

        # Create a map from the joint name to its control info
        self._jointname2jointcontrolinfo = dict()

        # Initialize all the joints in POSITION mode
        for name in self.joint_names():
            self._jointname2jointcontrolinfo[name] = JointControlInfo(
                mode=JointControlMode.POSITION)
            ok_mode = self.set_joint_control_mode(name, JointControlMode.POSITION)
            assert ok_mode, \
                "Failed to initialize the control mode of joint '{}'".format(name)
Ejemplo n.º 8
0
def test_joint_controller():
    # ==========
    # PARAMETERS
    # ==========

    agent_rate = 100.0
    physics_rate = 1000.0
    controller_rate = 500.0

    plugin_data = bindings.PluginData()
    plugin_data.libName = "RobotController"
    plugin_data.className = "gympp::plugins::RobotController"

    # Find and load the model SDF file
    model_sdf_file = resource_finder.find_resource("CartPole/CartPole.urdf")
    with open(model_sdf_file, "r") as stream:
        model_sdf_string = stream.read()

    # Initialize the model data
    model_data = bindings.ModelInitData()
    model_data.sdfString = model_sdf_string

    # Get the model name
    model_name = bindings.GazeboWrapper.getModelNameFromSDF(model_sdf_string)
    robot_name = model_name

    # =============
    # CONFIGURATION
    # =============

    # Create the gazebo wrapper
    num_of_iterations = int(physics_rate / agent_rate)
    desired_rtf = float(np.finfo(np.float32).max)
    gazebo = bindings.GazeboWrapper(num_of_iterations, desired_rtf, physics_rate)
    assert gazebo, "Failed to get the gazebo wrapper"

    # Set the verbosity
    logger.set_level(gym.logger.DEBUG)

    # Initialize the world
    world_ok = gazebo.setupGazeboWorld("DefaultEmptyWorld.world")
    assert world_ok, "Failed to initialize the gazebo world"

    # Initialize the ignition gazebo wrapper (creates a paused simulation)
    gazebo_initialized = gazebo.initialize()
    assert gazebo_initialized, "Failed to initialize ignition gazebo"

    # Insert the model
    model_ok = gazebo.insertModel(model_data, plugin_data)
    assert model_ok, "Failed to insert the model in the simulation"

    assert bindings.RobotSingleton_get().exists(robot_name), \
        "The robot interface was not registered in the singleton"

    # Extract the robot interface from the singleton
    robot_weak_ptr = bindings.RobotSingleton_get().getRobot(robot_name)
    assert not robot_weak_ptr.expired(), "The Robot object has expired"
    assert robot_weak_ptr.lock(), \
        "The returned Robot object does not contain a valid interface"
    assert robot_weak_ptr.lock().valid(), "The Robot object is not valid"

    # Get the pointer to the robot interface
    robot = robot_weak_ptr.lock()

    # Set the default update rate
    robot.setdt(1 / controller_rate)

    # Control the cart joint in position
    ok_cm = robot.setJointControlMode("linear", bindings.JointControlMode_Position)
    assert ok_cm, "Failed to control the cart joint in position"

    # Set the PID of the cart joint
    pid = bindings.PID(10000, 1000, 1000)
    pid_ok = robot.setJointPID("linear", pid)
    assert pid_ok, "Failed to set the PID of the cart joint"

    # Reset the robot state
    robot.resetJoint("pivot", 0, 0)
    robot.resetJoint("linear", 0, 0)

    # Generate the cart trajectory
    cart_ref = np.fromiter(
        (0.2 * np.sin(2 * np.pi * 0.5 * t) for t in np.arange(0, 5, 1 / agent_rate)),
        dtype=np.float)

    # Initialize the cart position buffer
    pos_cart_buffer = np.zeros(np.shape(cart_ref))

    for (i, ref) in enumerate(cart_ref):
        # Set the references
        ok1 = robot.setJointPositionTarget("linear", ref)
        assert ok1, "Failed to set joint references"

        # Step the simulator
        gazebo.run()

        # Read the position
        pos_cart_buffer[i] = robot.jointPosition("linear")

    # Check that the trajectory was followed correctly
    assert np.abs(pos_cart_buffer - cart_ref).sum() / cart_ref.size < 5E-3, \
        "The reference trajectory was not tracked correctly"
Ejemplo n.º 9
0
def get_gazebo_and_robot(rtf: float,
                         physics_rate: float,
                         agent_rate: float,
                         # controller_rate: float = None,
                         ) \
        -> Tuple[bindings.GazeboWrapper, bindings.Robot]:

    # ==========
    # PARAMETERS
    # ==========

    model_sdf = "CartPole/CartPole.sdf"
    world_sdf = "DefaultEmptyWorld.world"

    plugin_data = bindings.PluginData()
    plugin_data.setLibName("RobotController")
    plugin_data.setClassName("gympp::plugins::RobotController")

    # Find and load the model SDF file
    model_sdf_file = resource_finder.find_resource(model_sdf)
    with open(model_sdf_file, "r") as stream:
        model_sdf_string = stream.read()

    # Initialize the model data
    model_data = bindings.ModelInitData()
    model_data.setSdfString(model_sdf_string)

    # Create a unique model name
    letters_and_digits = string.ascii_letters + string.digits
    prefix = ''.join(random.choice(letters_and_digits) for _ in range(6))

    # Get the model name
    model_name = bindings.GazeboWrapper.getModelNameFromSDF(model_sdf_string)
    model_data.modelName = prefix + "::" + model_name
    robot_name = model_data.modelName

    num_of_iterations_per_gazebo_step = physics_rate / agent_rate
    assert num_of_iterations_per_gazebo_step == int(
        num_of_iterations_per_gazebo_step)

    # =============
    # CONFIGURATION
    # =============

    # Create the gazebo wrapper
    gazebo = bindings.GazeboWrapper(int(num_of_iterations_per_gazebo_step),
                                    rtf, physics_rate)
    assert gazebo, "Failed to get the gazebo wrapper"

    # Set verbosity
    logger.set_level(gym.logger.DEBUG)

    # Initialize the world
    world_ok = gazebo.setupGazeboWorld(world_sdf)
    assert world_ok, "Failed to initialize the gazebo world"

    # Initialize the ignition gazebo wrapper (creates a paused simulation)
    gazebo_initialized = gazebo.initialize()
    assert gazebo_initialized, "Failed to initialize ignition gazebo"

    # Insert the model
    model_ok = gazebo.insertModel(model_data, plugin_data)
    assert model_ok, "Failed to insert the model in the simulation"

    assert bindings.RobotSingleton_get().exists(robot_name), \
        "The robot interface was not registered in the singleton"

    # Extract the robot interface from the singleton
    robot_weak_ptr = bindings.RobotSingleton_get().getRobot(robot_name)
    assert not robot_weak_ptr.expired(), "The Robot object has expired"
    assert robot_weak_ptr.lock(), \
        "The returned Robot object does not contain a valid interface"
    assert robot_weak_ptr.lock().valid(), "The Robot object is not valid"

    # Get the pointer to the robot interface
    robot = robot_weak_ptr.lock()

    # Set the joint controller period
    robot.setdt(0.001 if physics_rate < 1000 else physics_rate)

    # Return the simulator and the robot object
    return gazebo, robot
Ejemplo n.º 10
0
    def gympp_robot(self) -> bindings.Robot:
        if self._gympp_robot:
            return self._gympp_robot

        # Find and load the model SDF file
        sdf_file = resource_finder.find_resource(self.model_file)
        with open(sdf_file, "r") as stream:
            sdf_string = stream.read()

        # Get the model name
        original_name = bindings.GazeboWrapper.getModelNameFromSDF(sdf_string)
        assert original_name, f"Failed to get model name from file {self.model_file}"

        # Create a unique robot name
        self._robot_name = self._prefix + "::" + original_name

        initial_base_pose, initial_base_orientation = self.initial_base_pose()

        # Initialize the model data
        model_data = bindings.ModelInitData()
        model_data.modelName = self._robot_name
        model_data.sdfString = sdf_string
        model_data.fixedPose = not self.is_floating_base()
        model_data.position = initial_base_pose.tolist()
        model_data.orientation = initial_base_orientation.tolist()

        if self._base_frame is not None:
            model_data.baseLink = self._base_frame

        # Initialize robot controller plugin
        plugin_data = bindings.PluginData()
        plugin_data.libName = "RobotController"
        plugin_data.className = "gympp::plugins::RobotController"

        # Insert the model
        ok_model = self._gazebo.insertModel(model_data, plugin_data)
        assert ok_model, "Failed to insert the model"

        # Extract the robot from the singleton
        gympp_robot_weak = bindings.RobotSingleton_get().getRobot(
            self._robot_name)

        # The robot is a weak pointer. Check that it is valid.
        assert not gympp_robot_weak.expired(), "The Robot object has expired"
        assert gympp_robot_weak.lock(), \
            "The returned Robot object does not contain a valid interface"

        self._gympp_robot = gympp_robot_weak.lock()
        assert self._gympp_robot.valid(), "The Robot object is not valid"

        if self._controller_rate is not None:
            logger.debug(f"Robot controller rate: {self._controller_rate} Hz")
            ok_dt = self._gympp_robot.setdt(1 / self._controller_rate)
            assert ok_dt, "Failed to set the robot controller period"

        s0 = self.initial_joint_positions()
        sdot0 = self.initial_joint_velocities()
        joint_names = list(self._gympp_robot.jointNames())

        assert s0.size == len(joint_names)
        assert sdot0.size == len(joint_names)

        for idx, name in enumerate(joint_names):
            ok_reset = self._gympp_robot.resetJoint(name, s0[idx], sdot0[idx])
            assert ok_reset, f"Failed to initialize the state of joint '{name}'"

        logger.debug(
            f"GazeboRobot '{self._gympp_robot.name()}' added to the simulation"
        )
        return self._gympp_robot