Python Tools.jacobian方法代码示例

# 需要导入模块: from tools import Tools [as 别名]
# 或者: from tools.Tools import jacobian [as 别名]
    def thrust_jacobian(self, alpha_0, u_0):
        '''Evaluate the jacobian of 
            J: f(v) = B(alpha) * u 
        at u_0, alpha_0, 
        '''
        def thrust_function(alpha):
            return np.dot(self.thrust_matrix(alpha), u_0)

        return Tools.jacobian(thrust_function, alpha_0)

# 需要导入模块: from tools import Tools [as 别名]
# 或者: from tools.Tools import jacobian [as 别名]
    def map_thruster(self, fx_des, fy_des, m_des, alpha_0, u_0, test_plot=False):
        '''Compute the optimal thruster configuration for Propagator using the Fossen method
        To make this work, alpha_0 and u_0 must be varied

        u_0 -> Root force
        alpha_0 -> Root angle (initial angle)

        We compute based on the minimization of the following nonlinear cost function:

        power(u) + s.T * G * s + (delta_theta).T * Q * (delta_theta) + (q / det(B(theta) * B(theta).T)) [1]

        It turns out that this problem is approximately convex in the range of allowable delta_theta
            so that if we linearize the problem, we can find an acceptable accurate minimum

        It is due to this linearization that we are computing over the FOTA and using u_0 + delta_u 
            instead of u directly.

        Arguments:
            fx_des, fy_des - X and Y components of desired force
            m_des - Desired torque about the Z axis
            alpha_0 - Current or initial thruster orientations (angle from the x-axis, positive left)
            u_0 - Current or initial thruster efforts
            test_plot - This should always be false in practice. Used to visually test for objective convexity

        Glossary of Terms:
            FOTA - First Order Taylor Approximation
            power(u) - Cost for the power consumption of a thruster exerting an effort, u
            Tau - Desired action (Fx, Fy, Torque about Z)
            Underactuated - B (The control-input-matrix, or thruster_matrix) is noninvertible
            B, or thrust_matrix - Matrix that maps a control input, "u" at a particular "alpha" 
                to a net force and torque experienced by the boat
            Singularity - when B becomes singular (noninvertible), we try to avoid 
                thruster configurations where the boat becomes underactuated

        Glossary of Variables:
            s - Control error (What the boat is doing minus Tau), you want this to be zero
            alpha - vector of angles
            alpha_0 - vector of current thruster orientations
            d_alpha - vector of angle changes
            u - vector of thruster forces
            u_0 - vector of current thruster forces
            d_u - vector of force changes
            q - boat manueverability constant
            epsilon - Used to avoid numerical issues when dividing by numbers near zero


        Bibliography:
            [1] Tor A. Johansen, Thor I. Fossen, and Svein P. Berge, 
                "Constrained Nonlinear Control Allocation With Singularity 
                    Avoidance Using Sequential Quadratic Programming",
                See: http://www.fossen.biz/home/papers/tcst04.pdf

            [2] Darsan Patel, Daniel Frank, and Dr. Carl Crane, 
                "Controlling an Overactuated Vehicle with Application to an Autonomous
                    Surface Vehicle Utilizing Azimuth Thrusters",
                See: http://bit.ly/1ayOdlg

        Author: Jacob Panikulam

        '''
        # Convert to numpy arrays
        alpha_0 = np.array(alpha_0)
        u_0 = np.array(u_0)

        # Desired
        tau = np.array([fx_des, fy_des, m_des])

        # Linearizations
        d_singularity = Tools.jacobian(self.singularity_avoidance, pt=alpha_0, order=3, dx=0.01)
        dBu_dalpha = self.thrust_jacobian(alpha_0, u_0)
        d_power = self.power_cost_scale

        B = self.thrust_matrix(alpha_0)

        def get_s((delta_angle, delta_u)):
            '''Equality constraint
            'S' is the force/torque error

                s + B(alpha_0) * delta_u + jac(B*u)*delta_alpha 
                    = -tau - B(alpha_0) * u_0
            -->   
            
                s = (-B(alpha_0) * delta_u) - jac(B*u)*delta_alpha - tau - B*alpha_0 *u_0
            '''
            s = -np.dot(B, delta_u) - np.dot(dBu_dalpha, delta_angle) + tau - np.dot(B, u_0)
            return s

        def objective((delta_angle_1, delta_angle_2, delta_u_1, delta_u_2)):
            '''Objective function for minimization'''

            delta_u = np.array([delta_u_1, delta_u_2])

            delta_angle = np.array([delta_angle_1, delta_angle_2])
            s = get_s((delta_angle, delta_u))

            # Sub-costs
            # power = np.sum(d_power * np.power(u_0 + delta_u, 2))
            # power = np.sum(d_power * np.power(delta_u, 2))
            power = 0

#.........这里部分代码省略.........