pyrobolearn/pyrobolearn/robots/techpod.py

#!/usr/bin/env python
"""Provide the techpod platform.
"""

import os
import numpy as np

from pyrobolearn.robots.uav import FixedWingUAV
from pyrobolearn.utils.transformation import get_matrix_from_quaternion
from pyrobolearn.utils.units import inches_to_meters


__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"


# TODO: several methods have to be moved in the `uav.py` file.
class Techpod(FixedWingUAV):
    r"""Techpod UAV

    Four forces (thrust, lift, drag, and gravity) act on an airplane.

    WARNING: Currently, in pybullet there is no air, so we simulate the lift, thrust, and drag forces. The gravity
    is carried out by pybullet.

    References:
        - [1] https://github.com/ethz-asl/rotors_simulator
        - [2] "Theory of flight": web.mit.edu/16.00/www/aec/flight.html
        - [3] "NASA: Guided Tours of the BGA": www.grc.nasa.gov/WWW/k-12/airplane/guided.html
    """

    def __init__(self, simulator, position=(0, 0, 0.5), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                 urdf=os.path.dirname(__file__) + '/urdfs/techpod/techpod.urdf'):
        """
        Initialize the Techpod robot.

        Args:
            simulator (Simulator): simulator instance.
            position (np.array[float[3]]): Cartesian world position.
            orientation (np.array[float[4]]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
            fixed_base (bool): if True, the robot base will be fixed in the world.
            scale (float): scaling factor that is used to scale the robot.
            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
        """
        # check parameters
        if position is None:
            position = (0., 0., 0.5)
        if len(position) == 2:  # assume x, y are given
            position = tuple(position) + (0.5,)
        if orientation is None:
            orientation = (0, 0, 0, 1)
        if fixed_base is None:
            fixed_base = False

        super(Techpod, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
        self.name = 'techpod'

        # info
        self.gravity = 9.81
        self.air_density = 1.225

        # change dynamics
        self.sim.change_dynamics(body_id=self.id, link_id=0, lateral_friction=0.)

        # from urdf
        self.radius = 0.15  # 0.14
        self.diameter = 2. * self.radius
        self.area = np.pi * self.radius**2

        self.max_velocity = 770  # rad/sec

        # Propeller pitches are around 0.0762m (3 inches) and 0.127m (5 inches)
        # (from https://www.dronezon.com/learn-about-drones-quadcopters/how-a-quadcopter-works-with-propellers-and\
        # -motors-direction-design-explained/)
        # The larger the prop (either increasing diameter, or pitch or both), the more energy it takes to spin it.
        # When buying propellers, they follow the syntax `LxP` where `L` is the length/diameter of the propeller
        # (in inches) and `P` is the propeller pitch (also in inches).
        # 1 inch = 0.0254m --> 0.28m = 11 inches
        # The value of the pitch below has been set by looking online for quadcopter props with 11 inches of length
        self.propeller_pitch = inches_to_meters(7)  # 4.7) # inches_to_meters(5.)

        # some constants
        self.k1 = 1./3.29546
        self.k2 = 1.5

    def calculate_thrust_force(self, angular_speed, area, propeller_pitch, v0=0, air_density=1.225):
        r"""
        Calculate the thrust force generated by the propeller (based on [6]).

        Args:
            angular_speed (float): angular speed of the propeller [rad/s]. If RPM, convert it to rad/s using the
                formula :math:`1 RPM = \frac{2\pi}{60} rad/s`.
            area (float): area of the propeller [m^2]
            propeller_pitch (float): "distance a propeller would move in 1 revolution if it were moving through a
                soft solid" [m]
            air_density (float): density of air [kg/m^3]. By default, it is the density of the air at sea level and at
                15 degrees Celsius. Note that this varies with the temperature, humidity, and pressure. It decreases
                with increasing altitude.

        Returns:
            float: thrust force generated by the propeller [N]

        References:
            [1] "Thrust Equation": https://www.grc.nasa.gov/WWW/k-12/airplane/thrsteq.html
            [2] "Propeller Thrust": https://www.grc.nasa.gov/WWW/k-12/airplane/propth.html
            [3] "Propeller Analysis": https://www.grc.nasa.gov/WWW/k-12/airplane/propanl.html
        """
        tmp = angular_speed / (2*np.pi) * propeller_pitch
        diameter = (4. * area / np.pi)**0.5
        return air_density * area * (tmp**2 - tmp*v0) * (self.k1 * diameter / propeller_pitch)**self.k2

    def calculate_lift_force(self):
        r"""
        Calculate the lift force.

        Returns:
            float[3]: lift force (in the z direction)

        References:
            [1] "What is Lift?": https://www.grc.nasa.gov/WWW/k-12/airplane/lift1.html
            [2] "Lift Equation": https://www.grc.nasa.gov/WWW/k-12/airplane/lifteq.html
            [3] "Inclination Effects on Lift": https://www.grc.nasa.gov/WWW/k-12/airplane/incline.html
            [4] https://www.comsol.com/blogs/how-do-i-compute-lift-and-drag/
        """
        pass

    def calculate_drag_force(self):
        r"""
        Calculate the drag force.

        Returns:
            float[3]: drag force (in the -x direction)

        References:
            [1] "What is Drag?": https://www.grc.nasa.gov/WWW/k-12/airplane/drag1.html
            [2] "Drag Equation": https://www.grc.nasa.gov/WWW/k-12/airplane/drageq.html
            [3] https://www.comsol.com/blogs/how-do-i-compute-lift-and-drag/
        """
        pass

    def calculate_elevator_moment(self):
        r"""
        Calculate the moment due to the elevators [1].

        Returns:
            float[3]: resulting moment in the y direction

        Reference:
            [1] "Horizontal Stabilizer - Elevator": https://www.grc.nasa.gov/WWW/k-12/airplane/elv.html
            [2] "Aircraft Rotations": https://www.grc.nasa.gov/WWW/k-12/airplane/rotations.html
        """
        pass

    def calculate_aileron_moment(self):
        """
        Calculate the moment due to the ailerons that increases/decreases the lift force [1].

        Returns:
            float[3]: resulting moment in the x direction.

        References:
            [1] "Ailerons": https://www.grc.nasa.gov/WWW/k-12/airplane/alr.html
            [2] "Aircraft Rotations": https://www.grc.nasa.gov/WWW/k-12/airplane/rotations.html
        """
        pass

    def calculate_flap_moment(self):
        """
        Increase lift or drag.

        Returns:

        References:
            [1] "Flaps and Slats": https://www.grc.nasa.gov/WWW/k-12/airplane/flap.html
        """
        pass

    def calculate_rudder_moment(self):
        """
        Calculate the moment due to the rudder [1].

        Returns:
            float[3]: resulting moment (in the z direction)

        Reference:
            [1] "Vertical Stabilizer - Rudder": https://www.grc.nasa.gov/WWW/k-12/airplane/rud.html
            [2] "Aircraft Rotations": https://www.grc.nasa.gov/WWW/k-12/airplane/rotations.html
        """
        pass

    def set_propeller_velocities(self, velocities, max_velocity=True, forces=True):
        """
        Set the joint velocities and apply the thrust force on the propeller link corresponding to the given
        joint id(s).

        Args:
            velocities (float[4]): velocity of each propeller
            forces (float, np.float[N], None, bool): maximum motor torques / forces. If True, it will apply the
                default maximum force values.
            max_velocity (float, bool, None): if True, it will make sure that the given velocity(ies) are below their
                authorized maximum value(s) (inferred from the URDF, or set previously by the user). If you already
                did the check outside the method or if you don't want limits, set this variable to False.

        Returns:
            None
        """
        joint_id = 7
        # call parent method
        super(Techpod, self).set_joint_velocities(velocities, joint_id, forces, max_velocity)

        # calculate thrust force of the given joints, and apply it on the link
        if max_velocity and velocities > self.max_velocity:
            velocities = self.max_velocity

        # compute propeller speed v0
        state = self.sim.get_link_state(self.id, joint_id, compute_velocity=True)  # , compute_forward_kinematics=True)
        R = get_matrix_from_quaternion(state[1])
        linear_velocity = np.array(state[-2])
        propeller_up_vec = R.dot(np.array([0., 1., 0.]))
        v0 = linear_velocity.dot(propeller_up_vec)
        # v0 = 0    # static thrust

        # compute thrust
        f = self.calculate_thrust_force(velocities, self.area, self.propeller_pitch)  # , v0)

        # apply force in the simulation
        # f = 20
        self.apply_external_force(force=[0, 0, -f], link_id=joint_id, position=(0., 0., 0.))

    def apply_physics(self):
        pass


# Test
if __name__ == "__main__":
    from itertools import count
    from pyrobolearn.simulators import BulletSim
    from pyrobolearn.worlds import BasicWorld

    # Create simulator
    sim = BulletSim()

    # create world
    world = BasicWorld(sim)

    # create robot
    robot = Techpod(sim)

    # print information about the robot
    robot.print_info()

    # run simulation
    for i in count():
        robot.set_propeller_velocities(100)
        # step in simulation
        world.step(sleep_dt=1./240)