Merge pull request #2 from shaoyifei96/main

Added method for computing reference angular acceleration, force, torque and rotor speeds

rotorpy/controllers/quadrotor_control.py

@@ -63,6 +63,105 @@ class SE3Control(object):
         self.f_to_TM = np.vstack((np.ones((1,self.num_rotors)),np.hstack([np.cross(self.rotor_pos[key],np.array([0,0,1])).reshape(-1,1)[0:2] for key in self.rotor_pos]), np.array([k*(-1)**i for i in range(self.num_rotors)]).reshape(1,-1)))
         self.TM_to_f = np.linalg.inv(self.f_to_TM)
 
+    def update_ref(self, t, flat_output):
+        """
+        This function receives the current time, and desired flat
+        outputs. It returns the reference command inputs.
+        Follows https://repository.upenn.edu/edissertations/547/
+
+        Inputs:
+            t, present time in seconds
+            flat_output, a dict describing the present desired flat outputs with keys
+                x,        position, m
+                x_dot,    velocity, m/s
+                x_ddot,   acceleration, m/s**2  a
+                x_dddot,  jerk, m/s**3          a_dot
+                x_ddddot, snap, m/s**4          a_ddot
+                yaw,      yaw angle, rad
+                yaw_dot,  yaw rate, rad/s
+                yaw_ddot, yaw acceleration, rad/s**2  #required! not the same if computing command using controller
+
+        Outputs:
+            control_input, a dict describing the present computed control inputs with keys
+                cmd_motor_speeds, rad/s
+                cmd_thrust, N (for debugging and laboratory; not used by simulator)
+                cmd_moment, N*m (for debugging; not used by simulator)
+                cmd_q, quaternion [i,j,k,w] (for laboratory; not used by simulator)
+                cmd_w, angular velocity
+                cmd_a, angular acceleration
+        """
+        cmd_motor_speeds = np.zeros((4,))
+        cmd_q = np.zeros((4,))
+
+        def normalize(x):
+            """Return normalized vector."""
+            return x / np.linalg.norm(x)
+
+        # def vee_map(S):
+        #     """Return vector corresponding to given skew symmetric matrix."""
+        #     return np.array([-S[1,2], S[0,2], -S[0,1]])
+
+        # Desired force vector.
+        t = flat_output['x_ddot']+ np.array([0, 0, self.g])
+        b3 = normalize(t) 
+        F_des = self.mass * (t)# this is vectorized
+
+        # Desired thrust is force projects onto b3 axis.
+        # R = Rotation.from_quat(state['q']).as_matrix() #this is where most of the problem is, there is no error in rotation!
+        # b3 = R @ np.array([0, 0, 1])
+        u1 = np.dot(F_des, b3)
+
+        # Desired orientation to obtain force vector.
+        b3_des = normalize(F_des) #b3_des and b3 are the same
+        yaw_des = flat_output['yaw']
+        c1_des = np.array([np.cos(yaw_des), np.sin(yaw_des), 0])
+        b2_des = normalize(np.cross(b3_des, c1_des))
+        b1_des = np.cross(b2_des, b3_des)
+        R_des = np.stack([b1_des, b2_des, b3_des]).T
+
+        R = R_des# assume we have perfect tracking on rotation
+        # Orientation error.
+        # S_err = 0.5 * (R_des.T @ R - R.T @ R_des)
+        # att_err = vee_map(S_err)
+        
+        # Following section follows Mellinger paper to compute reference angular velocity
+        dot_u1 = np.dot(b3,flat_output['x_dddot'])
+        hw = self.mass/u1*(flat_output['x_dddot']-dot_u1*b3)
+        p  = np.dot(-hw, b2_des)
+        q  = np.dot(hw, b1_des)
+        w_des = np.array([0, 0, flat_output['yaw_dot']])
+        r  = np.dot(w_des, b3_des)
+        Omega = np.array([p, q, r])
+
+        wwu1b3 = np.cross(Omega, np.cross(Omega, u1*b3))
+        ddot_u1 = np.dot(b3, self.mass*flat_output['x_ddddot']) - np.dot(b3, wwu1b3)
+        ha = 1.0/u1*(self.mass*flat_output['x_ddddot'] - ddot_u1*b3 - 2*np.cross(Omega,dot_u1*b3) - wwu1b3)
+        p_dot = np.dot(-ha, b2_des)
+        q_dot = np.dot(ha, b1_des)
+        np.cross(Omega, Omega)
+        r_dot = flat_output['yaw_ddot'] *np.dot(np.array([0,0,1.0]), b3_des) #uniquely need yaw_ddot
+        Alpha = np.array([p_dot, q_dot, r_dot]) 
+
+
+
+        u2 =  self.inertia @ Alpha + np.cross(Omega, self.inertia @ Omega)
+        # print(u1,u2)
+        TM = np.array([u1, u2[0], u2[1], u2[2]])
+        cmd_motor_forces = self.TM_to_f @ TM
+        cmd_motor_speeds = cmd_motor_forces / self.k_eta
+        cmd_motor_speeds = np.sign(cmd_motor_speeds) * np.sqrt(np.abs(cmd_motor_speeds))
+
+        cmd_q = Rotation.from_matrix(R_des).as_quat()
+
+
+        control_input = {'cmd_motor_speeds':cmd_motor_speeds,
+                        'cmd_thrust':u1,
+                        'cmd_moment':u2,
+                        'cmd_q':cmd_q,
+                        'cmd_w':Omega,
+                        'cmd_a':Alpha}
+        return control_input
+    
     def update(self, t, state, flat_output):
         """
         This function receives the current time, true state, and desired flat

rotorpy/simulate.py

@@ -98,6 +98,7 @@ def simulate(world, initial_state, vehicle, controller, trajectory, wind_profile
         control = [sanitize_control_dic(controller.update(time[-1], mocap_measurements[-1], flat[-1]))]
     else:
         control = [sanitize_control_dic(controller.update(time[-1], state[-1], flat[-1]))]
+        control_ref = [sanitize_control_dic(controller.update_ref(time[-1], flat[-1]))]
     state_dot =  vehicle.statedot(state[0], control[0]['cmd_motor_speeds'], t_step)
     imu_measurements.append(imu.measurement(state[-1], state_dot, with_noise=True))
     imu_gt.append(imu.measurement(state[-1], state_dot, with_noise=False))

rotorpy/trajectories/circular_traj.py

@@ -113,12 +113,15 @@ class CircularTraj(object):
         if self.yaw_bool:
             yaw = np.pi/4*np.sin(np.pi*t)
             yaw_dot = np.pi*np.pi/4*np.cos(np.pi*t)
+            yaw_ddot = -np.pi*np.pi*np.pi/4*np.sin(np.pi*t)
+
         else:
             yaw = 0
             yaw_dot = 0
+            yaw_ddot = 0
 
         flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
-                        'yaw':yaw, 'yaw_dot':yaw_dot}
+                        'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
         return flat_output
 
 class ThreeDCircularTraj(object):