From 7c8ebc1413d7e2b0daa26a8b8aaf717172b99f61 Mon Sep 17 00:00:00 2001 From: spencerfolk Date: Thu, 14 Dec 2023 16:18:05 -0500 Subject: [PATCH] Accept new control abstractions besides cmd motor speeds --- rotorpy/simulate.py | 6 +- rotorpy/vehicles/multirotor.py | 137 +++++++++++++++++++++++++++++++-- 2 files changed, 134 insertions(+), 9 deletions(-) diff --git a/rotorpy/simulate.py b/rotorpy/simulate.py index 6f89876..f1081f2 100644 --- a/rotorpy/simulate.py +++ b/rotorpy/simulate.py @@ -98,7 +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]))] - state_dot = vehicle.statedot(state[0], control[0]['cmd_motor_speeds'], t_step) + state_dot = vehicle.statedot(state[0], control[0], 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)) state_estimate.append(estimator.step(state[0], control[0], imu_measurements[0], mocap_measurements[0])) @@ -113,7 +113,7 @@ def simulate(world, initial_state, vehicle, controller, trajectory, wind_profile break time.append(time[-1] + t_step) state[-1]['wind'] = wind_profile.update(time[-1], state[-1]['x']) - state.append(vehicle.step(state[-1], control[-1]['cmd_motor_speeds'], t_step)) + state.append(vehicle.step(state[-1], control[-1], t_step)) flat.append(sanitize_trajectory_dic(trajectory.update(time[-1]))) mocap_measurements.append(mocap.measurement(state[-1], with_noise=True, with_artifacts=mocap.with_artifacts)) state_estimate.append(estimator.step(state[-1], control[-1], imu_measurements[-1], mocap_measurements[-1])) @@ -121,7 +121,7 @@ def simulate(world, initial_state, vehicle, controller, trajectory, wind_profile control.append(sanitize_control_dic(controller.update(time[-1], mocap_measurements[-1], flat[-1]))) else: control.append(sanitize_control_dic(controller.update(time[-1], state[-1], flat[-1]))) - state_dot = vehicle.statedot(state[-1], control[-1]['cmd_motor_speeds'], t_step) + state_dot = vehicle.statedot(state[-1], control[-1], 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)) diff --git a/rotorpy/vehicles/multirotor.py b/rotorpy/vehicles/multirotor.py index d3080cc..78bf799 100644 --- a/rotorpy/vehicles/multirotor.py +++ b/rotorpy/vehicles/multirotor.py @@ -32,7 +32,19 @@ def quat_dot(quat, omega): class Multirotor(object): """ - Quadrotor forward dynamics model. + Multirotor forward dynamics model. + + states: [position, velocity, attitude, body rates, wind, rotor speeds] + + Parameters: + quad_params: a dictionary containing relevant physical parameters for the multirotor. + initial_state: the initial state of the vehicle. + control_abstraction: the appropriate control abstraction that is used by the controller, options are... + 'cmd_motor_speed': the controller directly commands motor speeds. + 'cmd_motor_force': the controller commands forces for each rotor. + 'cmd_ctbr': the controller commands a collective thrsut and body rates. + 'cmd_ctbm': the controller commands a collective thrust and moments on the x/y/z body axes + 'cmd_vel': the controller commands a velocity vector in the body frame. """ def __init__(self, quad_params, initial_state = {'x': np.array([0,0,0]), 'v': np.zeros(3,), @@ -40,6 +52,7 @@ class Multirotor(object): 'w': np.zeros(3,), 'wind': np.array([0,0,0]), # Since wind is handled elsewhere, this value is overwritten 'rotor_speeds': np.array([1788.53, 1788.53, 1788.53, 1788.53])}, + control_abstraction='cmd_motor_speeds', ): """ Initialize quadrotor physical parameters. @@ -95,9 +108,24 @@ class Multirotor(object): self.inv_inertia = inv(self.inertia) self.weight = np.array([0, 0, -self.mass*self.g]) + # Control allocation + k = self.k_m/self.k_eta # Ratio of torque to thrust coefficient. + + # Below is an automated generation of the control allocator matrix. It assumes that all thrust vectors are aligned + # with the z axis and that the "sign" of each rotor yaw moment alternates starting with positive for r1. + 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]), (k * self.rotor_dir).reshape(1,-1))) + self.TM_to_f = np.linalg.inv(self.f_to_TM) + # Set the initial state self.initial_state = initial_state + self.control_abstraction = control_abstraction + + self.k_w = 1 # The body rate P gain (for cmd_ctbr) + self.k_v = 10 # The *world* velocity P gain (for cmd_vel) + self.kp_att = 544 # The attitude P gain (for cmd_vel and cmd_ctatt) + self.kd_att = 46.64 # The attitude D gain (for cmd_vel and cmd_ctatt) + def extract_geometry(self): """ Extracts the geometry in self.rotors for efficient use later on in the computation of @@ -113,13 +141,15 @@ class Multirotor(object): return - def statedot(self, state, cmd_rotor_speeds, t_step): + def statedot(self, state, control, t_step): """ Integrate dynamics forward from state given constant cmd_rotor_speeds for time t_step. """ + cmd_rotor_speeds = self.get_cmd_motor_speeds(state, control) + # The true motor speeds can not fall below min and max speeds. - cmd_rotor_speeds = np.clip(cmd_rotor_speeds, self.rotor_speed_min, self.rotor_speed_max) + cmd_rotor_speeds = np.clip(cmd_rotor_speeds, self.rotor_speed_min, self.rotor_speed_max) # Form autonomous ODE for constant inputs and integrate one time step. def s_dot_fn(t, s): @@ -134,13 +164,15 @@ class Multirotor(object): return state_dot - def step(self, state, cmd_rotor_speeds, t_step): + def step(self, state, control, t_step): """ - Integrate dynamics forward from state given constant cmd_rotor_speeds for time t_step. + Integrate dynamics forward from state given constant control for time t_step. """ + cmd_rotor_speeds = self.get_cmd_motor_speeds(state, control) + # The true motor speeds can not fall below min and max speeds. - cmd_rotor_speeds = np.clip(cmd_rotor_speeds, self.rotor_speed_min, self.rotor_speed_max) + cmd_rotor_speeds = np.clip(cmd_rotor_speeds, self.rotor_speed_min, self.rotor_speed_max) # Form autonomous ODE for constant inputs and integrate one time step. def s_dot_fn(t, s): @@ -256,6 +288,99 @@ class Multirotor(object): return (FtotB, MtotB) + def get_cmd_motor_speeds(self, state, control): + """ + Computes the commanded motor speeds depending on the control abstraction. + For higher level control abstractions, we have low-level controllers that will produce motor speeds based on the higher level commmand. + + """ + + if self.control_abstraction == 'cmd_motor_speeds': + # The controller directly controls motor speeds, so command that. + return control['cmd_motor_speeds'] + + elif self.control_abstraction == 'cmd_motor_thrusts': + # The controller commands individual motor forces. + cmd_motor_speeds = control['cmd_motor_thrusts'] / self.k_eta # Convert to motor speeds from thrust coefficient. + return np.sign(cmd_motor_speeds) * np.sqrt(np.abs(cmd_motor_speeds)) + + elif self.control_abstraction == 'cmd_ctbm': + # The controller commands collective thrust and moment on each axis. + cmd_thrust = control['cmd_thrust'] + cmd_moment = control['cmd_moment'] + + elif self.control_abstraction == 'cmd_ctbr': + # The controller commands collective thrust and body rates on each axis. + + cmd_thrust = control['cmd_thrust'] + + # First compute the error between the desired body rates and the actual body rates given by state. + w_err = state['w'] - control['cmd_w'] + + # Computed commanded moment based on the attitude error and body rate error + wdot_cmd = -self.k_w*w_err + cmd_moment = self.inertia@wdot_cmd + + # Now proceed with the cmd_ctbm formulation. + + elif self.control_abstraction == 'cmd_vel': + # The controller commands a velocity vector. + + # Get the error in the current velocity. + v_err = state['v'] - control['cmd_v'] + + # Get desired acceleration based on P control of velocity error. + a_cmd = -self.k_v*v_err + + # Get desired force from this acceleration. + F_des = self.mass*(a_cmd + np.array([0, 0, self.g])) + + R = Rotation.from_quat(state['q']).as_matrix() + b3 = R @ np.array([0, 0, 1]) + cmd_thrust = np.dot(F_des, b3) + + # Follow rest of SE3 controller to compute cmd moment. + + # Desired orientation to obtain force vector. + b3_des = F_des/np.linalg.norm(F_des) + c1_des = np.array([1, 0, 0]) + b2_des = np.cross(b3_des, c1_des)/np.linalg.norm(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 + + # Orientation error. + S_err = 0.5 * (R_des.T @ R - R.T @ R_des) + att_err = np.array([-S_err[1,2], S_err[0,2], -S_err[0,1]]) + + # Angular control; vector units of N*m. + cmd_moment = self.inertia @ (-self.kp_att*att_err - self.kd_att*state['w']) + np.cross(state['w'], self.inertia@state['w']) + + elif self.control_abstraction == 'cmd_ctatt': + # The controller commands the collective thrust and attitude. + + cmd_thrust = control['cmd_thrust'] + + # Compute the shape error from the current attitude and the desired attitude. + R = Rotation.from_quat(state['q']).as_matrix() + R_des = Rotation.from_quat(control['cmd_q']).as_matrix() + + S_err = 0.5 * (R_des.T @ R - R.T @ R_des) + att_err = np.array([-S_err[1,2], S_err[0,2], -S_err[0,1]]) + + # Compute command moment based on attitude error. + cmd_moment = self.inertia @ (-self.kp_att*att_err - self.kd_att*state['w']) + np.cross(state['w'], self.inertia@state['w']) + + else: + raise ValueError("Invalid control abstraction selected. Options are: cmd_motor_speeds, cmd_motor_thrusts, cmd_ctbm, cmd_ctbr, cmd_ctatt, cmd_vel") + + # Take the commanded thrust and body moments and convert them to motor speeds + TM = np.concatenate(([cmd_thrust], cmd_moment)) # Concatenate thrust and moment into an array + cmd_motor_forces = self.TM_to_f @ TM # Convert to cmd_motor_forces from allocation matrix + cmd_motor_speeds = cmd_motor_forces / self.k_eta # Convert to motor speeds from thrust coefficient. + cmd_motor_speeds = np.sign(cmd_motor_speeds) * np.sqrt(np.abs(cmd_motor_speeds)) + + return cmd_motor_speeds + @classmethod def rotate_k(cls, q): """