From 947df519eb42b6894942682933402792e588c158 Mon Sep 17 00:00:00 2001 From: astik <41.astiksrivastava@gmail.com> Date: Tue, 14 May 2024 13:17:44 +0530 Subject: [PATCH] minimum snap working (upto 3m/s) --- .../differential_flatness.py | 77 ++++++++++--------- .../differential_flatness/waypoints.py | 19 +++-- .../AC_AttitudeControl/AC_AttitudeControl.h | 3 + .../AC_CustomControl_INDI.cpp | 3 +- 4 files changed, 58 insertions(+), 44 deletions(-) diff --git a/Tools/ros2/differential_flatness/differential_flatness/differential_flatness.py b/Tools/ros2/differential_flatness/differential_flatness/differential_flatness.py index 3bd26234f61513..660896e7c22dc1 100644 --- a/Tools/ros2/differential_flatness/differential_flatness/differential_flatness.py +++ b/Tools/ros2/differential_flatness/differential_flatness/differential_flatness.py @@ -56,18 +56,17 @@ def velocity_callback(self, msg): def calculate_trajectory(self): current_time = self.get_clock().now().nanoseconds - # t = (current_time - self.start_time)/1e9 # converting from nanoseconds to seconds - # self.sDes = self.traj.desiredState(t,0.01) - # self.desPos = np.array([self.sDes[0], self.sDes[1], self.sDes[2]]) - # self.desVel = np.array([self.sDes[3], self.sDes[4], self.sDes[5]]) - # self.desAcc = np.array([self.sDes[6], self.sDes[7], self.sDes[8]]) - # self.desThr = self.sDes[9:12] - # self.desEul = self.sDes[12:15] - # self.desPQR = self.sDes[15:18] - # self.yawFF = self.sDes[18] - # self.desJerk = self.sDes[19:22] - # self.desSnap = self.sDes[22:25] - self.desPos = np.array([0, 0, 3]) + t = (current_time - self.start_time)/1e9 # converting from nanoseconds to seconds + self.sDes = self.traj.desiredState(t,0.01) + self.desPos = np.array([self.sDes[0], self.sDes[1], self.sDes[2]]) + self.desVel = np.array([self.sDes[3], self.sDes[4], self.sDes[5]]) + self.desAcc = np.array([self.sDes[6], self.sDes[7], self.sDes[8]]) + self.desThr = self.sDes[9:12] + self.desEul = self.sDes[12:15] + self.desPQR = self.sDes[15:18] + self.yawFF = self.sDes[18] + self.desJerk = self.sDes[19:22] + self.desSnap = self.sDes[22:25] def quat_mult(self,q, p): # Extract scalar and vector parts @@ -163,8 +162,8 @@ def normalize_vector(self, v): def acceleration_setpoint(self): vel = self.current_velocity() pos = self.current_position() - desired_pos = np.array([5, 10, -10]) - desired_vel = np.array([0, 0, 0]) + desired_pos = self.desPos + desired_vel = self.desVel pos_error = desired_pos - pos vel_error = desired_vel - vel acc_sp = np.zeros(3) @@ -172,14 +171,15 @@ def acceleration_setpoint(self): kd_x = 0.5 kp_y = kp_x kd_y = kd_x - kp_z = 1.5 - kd_z = 1.4 - ki_z = 0.2 + kp_z = 2.5 + kd_z = 2.0 + ki_z = 0.09 self.z_i += ki_z*pos_error[2]*self.Ts + self.z_i = np.clip(self.z_i, -1, 1) acc_sp[0] = kp_x*pos_error[0] + kd_x*vel_error[0] acc_sp[1] = kp_y*pos_error[1] + kd_y*vel_error[1] acc_sp[2] = kp_z*pos_error[2] + kd_z*vel_error[2] + self.z_i - 9.81 - + acc_sp = acc_sp + self.desAcc return acc_sp def normalized_thrust_setpoint(self, des_accn): @@ -202,7 +202,7 @@ def normalized_thrust_setpoint(self, des_accn): def desired_orientation(self, des_accn): # Create Full Desired Quaternion Based on Thrust Setpoint and Desired Yaw Angle # --------------------------- - yaw_sp = 0.0 + yaw_sp = self.desEul[2] # Desired body_z axis direction des_accn = -self.normalize_vector(des_accn) body_z = des_accn @@ -248,22 +248,24 @@ def quat_inverse(self,quat): def computeNominalReference(self): des_acceleration = self.acceleration_setpoint() des_orientation = self.desired_orientation(des_acceleration) - roll, pitch, yaw = self.quaternion_to_euler(des_orientation) - rollc, pitchc, yawc = self.quaternion_to_euler(self.current_orientation()) + thrust = self.normalized_thrust_setpoint(des_acceleration) + + #roll, pitch, yaw = self.quaternion_to_euler(des_orientation) + #rollc, pitchc, yawc = self.quaternion_to_euler(self.current_orientation()) #self.get_logger().info('current attitude: r: {:.2f} p: {:.2f} y: {:.2f}]'.format(rollc, pitchc, yawc)) - self.get_logger().info('desired acceleration x = {:,.2f}, y = {:,.2f}, z = {:,.2f}'.format(des_acceleration[0], des_acceleration[1], des_acceleration[2])) + #self.get_logger().info('desired acceleration x = {:,.2f}, y = {:,.2f}, z = {:,.2f}'.format(des_acceleration[0], des_acceleration[1], des_acceleration[2])) body_x = self.rotate_vector_by_quaternion(np.array([1, 0, 0]), des_orientation) body_y = self.rotate_vector_by_quaternion(np.array([0, 1, 0]), des_orientation) - body_z = self.rotate_vector_by_quaternion(np.array([0, 0, 1]), des_orientation) + body_z = self.rotate_vector_by_quaternion(np.array([0, 0, -1]), des_orientation) reference_heading = self.desEul[2] q_heading = np.array([math.cos(reference_heading/2), 0, 0, math.sin(reference_heading/2)]) x_C = self.rotate_vector_by_quaternion(np.array([1, 0, 0]), q_heading) y_C = self.rotate_vector_by_quaternion(np.array([0, 1, 0]), q_heading) - ref_acc = self.desAcc + self.gravity + ref_acc = des_acceleration pqr_sp = np.zeros(3) pqr_dot_sp = np.zeros(3) if(np.isclose(np.linalg.norm(ref_acc), 0)): @@ -272,11 +274,12 @@ def computeNominalReference(self): else: pqr_sp[0] = -1/np.linalg.norm(ref_acc)*np.dot(body_y,self.desJerk) pqr_sp[1] = 1/np.linalg.norm(ref_acc)*np.dot(body_x,self.desJerk) + if(np.isclose(np.linalg.norm(np.cross(y_C,body_z)), 0)): pqr_sp[2] = 0.0 else: pqr_sp[2] = 1/np.linalg.norm(np.cross(y_C,body_z))*(self.yawFF*np.dot(x_C,body_x) + pqr_sp[1]*np.dot(y_C,body_z)) - #print("ang rate setpoint",pqr_sp) + thrust_dot = np.dot(body_z,self.desJerk) if(np.isclose(np.linalg.norm(ref_acc), 0)): pqr_dot_sp[0] = 0.0 @@ -293,26 +296,30 @@ def computeNominalReference(self): - pqr_sp[0]*pqr_sp[2]*np.dot(y_C,body_z) + pqr_dot_sp[1]*np.dot(y_C,body_z)) - self.command_msg.angular_rate.x = 0.0 - self.command_msg.angular_rate.y = 0.0 - self.command_msg.angular_rate.z = 0.0 + self.command_msg.angular_rate.x = pqr_sp[0] + self.command_msg.angular_rate.y = pqr_sp[1] + self.command_msg.angular_rate.z = pqr_sp[2] self.command_msg.angular_acceleration.x = pqr_dot_sp[0] self.command_msg.angular_acceleration.y = pqr_dot_sp[1] self.command_msg.angular_acceleration.z = pqr_dot_sp[2] + + # self.command_msg.angular_rate.x = 0.0 + # self.command_msg.angular_rate.y = 0.0 + # self.command_msg.angular_rate.z = 0.0 + # self.command_msg.angular_acceleration.x = 0.0 + # self.command_msg.angular_acceleration.y = 0.0 + # self.command_msg.angular_acceleration.z = 0.0 + + + self.command_msg.orientation = Quaternion(x = des_orientation[0], y = des_orientation[1], z = des_orientation[2], w = des_orientation[3]) + self.command_msg.thrust = thrust def prepare_angular_vel_accn_message(self): # Prepare the AngularVelandAccn message self.calculate_trajectory() - des_acc = self.acceleration_setpoint() - orientation = self.desired_orientation(des_acc) - thrust = self.normalized_thrust_setpoint(des_acc) self.computeNominalReference() - self.command_msg.orientation = Quaternion(x = orientation[0], y = orientation[1], z = orientation[2], w = orientation[3]) - self.get_logger().info('desired orientation x = {:,.2f}, y = {:,.2f}, z = {:,.2f}, w = {:,.2f}'.format(orientation[0], orientation[1], orientation[2], orientation[3])) - - self.command_msg.thrust = thrust # Publish the AngularVelandAccn message self.publisher.publish(self.command_msg) #self.get_logger().info('Published command Message, thrust = {:.2f}'.format(thrust)) diff --git a/Tools/ros2/differential_flatness/differential_flatness/waypoints.py b/Tools/ros2/differential_flatness/differential_flatness/waypoints.py index 3add718b585b5d..a1e1753637e0f5 100644 --- a/Tools/ros2/differential_flatness/differential_flatness/waypoints.py +++ b/Tools/ros2/differential_flatness/differential_flatness/waypoints.py @@ -13,20 +13,23 @@ def makeWaypoints(): - v_average = 10 + v_average = 3 t_ini = 3 - t = np.array([5, 5, 5, 5]) + t = np.array([5, 5, 5, 5, 5, 5, 5]) wp_ini = np.array([0, 0, 0]) - wp = np.array([[0, 0, 10], - [0, 0, 20], - [0, 0, 30], - [0, 0, 40], - [0, 0, 50]]) + wp = np.array([[0, 0, -10], + [-9, 5, -10], + [-14, 0, -10], + [-9, -5, -10], + [9, 5, -10], + [14, 0, -10], + [9, -5, -10], + [0, 0, -11]]) yaw_ini = 0 - yaw = np.array([0, 0, 0, 0, 0]) + yaw = np.array([0, 0, 0, 0, 0, 0, 0, 0]) t = np.hstack((t_ini, t)).astype(float) wp = np.vstack((wp_ini, wp)).astype(float) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 5ab601b31c9040..b1a6278d95fef0 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -224,6 +224,9 @@ class AC_AttitudeControl { // Return the angular velocity of the target (setpoint) [rad/s] in the target attitude frame const Vector3f& get_attitude_target_ang_vel() const { return _ang_vel_target;} + // Return the target angular acceleration setpoint [rad/s^2] only for use with custom controllers + const Vector3f& get_attitude_target_ang_accn() const { return _ang_acceleration_target;} + // Return the angle between the target thrust vector and the current thrust vector. float get_att_error_angle_deg() const { return degrees(_thrust_error_angle); } diff --git a/libraries/AC_CustomControl/AC_CustomControl_INDI.cpp b/libraries/AC_CustomControl/AC_CustomControl_INDI.cpp index 75a71b6877157d..3c67c46cfe6b3a 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_INDI.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl_INDI.cpp @@ -155,6 +155,7 @@ Vector3f AC_CustomControl_INDI::update(void) // run custom controller after here Quaternion attitude_body, attitude_target; + Vector3f angular_acc_target = _att_control->get_attitude_target_ang_accn(); _ahrs->get_quat_body_to_ned(attitude_body); attitude_target = _att_control->get_attitude_target_quat(); Vector3f gyro_latest = _ahrs->get_gyro_latest(); @@ -167,7 +168,7 @@ Vector3f AC_CustomControl_INDI::update(void) Vector3f ang_vel_body_feedforward = rotation_target_to_body * _att_control->get_attitude_target_ang_vel(); Vector3f ang_vel_target = run_attitude_controller(attitude_target, attitude_body); - run_angvel_controller(ang_vel_target + ang_vel_body_feedforward, gyro_latest, Vector3f(0.0f, 0.0f, 0.0f)); + run_angvel_controller(ang_vel_target + ang_vel_body_feedforward, gyro_latest, angular_acc_target); // return what arducopter main controller outputted return Vector3f(_torque_cmd_scaled.x, _torque_cmd_scaled.y, _torque_cmd_scaled.z);