RLBench/envs/_quadrocopter/quadrotor_dynamics.py
def _forces_to_derivatives(self, forces):
        """Compute the state derivatives based on applied forces."""
        # Update position
        derivatives = State()

        derivatives.pos[:] = self.state.vel

        drag = self._compute_drag()

        # Update accelerations
        derivatives.acc = np.sum(forces) * self.state.R[:, 2] - drag

        # Add external forces
        for force in self.external_forces:
            derivatives.acc += force(self.state)

        # Normalize with mass and add gravity
        derivatives.acc /= self.params.m
        derivatives.acc[2] -= self.params.g

        # Update velocities
        derivatives.vel[:] = self.state.acc

        p, q, r = self.state.omega
        derivatives.R = self.state.R.dot(np.array([[0, -r, q],
                                                   [r, 0, -p],
                                                   [-q, p, 0]]))

        # Angular velocity changes
        f1, f2, f3, f4 = forces

        # p' = (1/Ix)*(L*(f2-f4) + (Iy-Iz)*r*q)
        p_dot = (self.params.L * (f2 - f4) +
                 (self.params.Iy - self.params.Iz) *
                 self.state.omega[2] * self.state.omega[1]) / self.params.Ix

        # q' = (1/Iy)*(L*(f3-f1) + (Iz-Ix)*r*p)
        q_dot = (self.params.L * (f3 - f1) +
                 (self.params.Iz - self.params.Ix) *
                 self.state.omega[2] * self.state.omega[0]) / self.params.Iy

        # r' = (1/Iz)*(K*(f1-f2+f3-f4) + (Ix-Iy)*p*q)
        r_dot = (self.params.K * (f1 - f2 + f3 - f4) +
                 (self.params.Ix - self.params.Iy) *
                 self.state.omega[0] * self.state.omega[1]) / self.params.Iz

        derivatives.omega = np.array([p_dot, q_dot, r_dot])

        return derivatives
Compute the state derivatives based on applied forces.
Compute the state derivatives based on applied forces.
Compute the state derivatives based on applied forces .
def _forces_to_derivatives(self, forces):
        derivatives = State()
        derivatives.pos[:] = self.state.vel
        drag = self._compute_drag()
        derivatives.acc = np.sum(forces) * self.state.R[:, 2] - drag
        for force in self.external_forces:
            derivatives.acc += force(self.state)
        derivatives.acc /= self.params.m
        derivatives.acc[2] -= self.params.g
        derivatives.vel[:] = self.state.acc
        p, q, r = self.state.omega
        derivatives.R = self.state.R.dot(np.array([[0, -r, q],
                                                   [r, 0, -p],
                                                   [-q, p, 0]]))
        f1, f2, f3, f4 = forces
        p_dot = (self.params.L * (f2 - f4) +
                 (self.params.Iy - self.params.Iz) *
                 self.state.omega[2] * self.state.omega[1]) / self.params.Ix
        q_dot = (self.params.L * (f3 - f1) +
                 (self.params.Iz - self.params.Ix) *
                 self.state.omega[2] * self.state.omega[0]) / self.params.Iy
        r_dot = (self.params.K * (f1 - f2 + f3 - f4) +
                 (self.params.Ix - self.params.Iy) *
                 self.state.omega[0] * self.state.omega[1]) / self.params.Iz
        derivatives.omega = np.array([p_dot, q_dot, r_dot])
        return derivatives
def _forces_to_derivatives ( self , forces ) : derivatives = State ( ) derivatives . pos [ : ] = self . state . vel drag = self . _compute_drag ( ) derivatives . acc = np . sum ( forces ) * self . state . R [ : , 2 ] - drag for force in self . external_forces : derivatives . acc += force ( self . state ) derivatives . acc /= self . params . m derivatives . acc [ 2 ] -= self . params . g derivatives . vel [ : ] = self . state . acc p , q , r = self . state . omega derivatives . R = self . state . R . dot ( np . array ( [ [ 0 , - r , q ] , [ r , 0 , - p ] , [ - q , p , 0 ] ] ) ) f1 , f2 , f3 , f4 = forces p_dot = ( self . params . L * ( f2 - f4 ) + ( self . params . Iy - self . params . Iz ) * self . state . omega [ 2 ] * self . state . omega [ 1 ] ) / self . params . Ix q_dot = ( self . params . L * ( f3 - f1 ) + ( self . params . Iz - self . params . Ix ) * self . state . omega [ 2 ] * self . state . omega [ 0 ] ) / self . params . Iy r_dot = ( self . params . K * ( f1 - f2 + f3 - f4 ) + ( self . params . Ix - self . params . Iy ) * self . state . omega [ 0 ] * self . state . omega [ 1 ] ) / self . params . Iz derivatives . omega = np . array ( [ p_dot , q_dot , r_dot ] ) return derivatives
Compute the state derivatives based on applied forces.
Compute the state derivatives based on applied forces .
"""Compute the state derivatives based on applied forces.""" # Update position # Update accelerations # Add external forces # Normalize with mass and add gravity # Update velocities # Angular velocity changes # p' = (1/Ix)*(L*(f2-f4) + (Iy-Iz)*r*q) # q' = (1/Iy)*(L*(f3-f1) + (Iz-Ix)*r*p) # r' = (1/Iz)*(K*(f1-f2+f3-f4) + (Ix-Iy)*p*q)
self
forces