i use the code below ,but with thrust curve like the picture, how to imporve it

import asyncio import rospy import math import time from tkinter import N from threading import Thread from matplotlib import markers from controller import PID

from scipy.spatial.transform import Rotation from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import TwistStamped

import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.mem import Poly4D from cflib.crazyflie.mem import MemoryElement from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils import uri_helper from cflib.crazyflie.commander import Commander from cflib.crtp.crtpstack import CRTPPacket from cflib.positioning.motion_commander import MotionCommander

Guidance import numpy as np import math as m import numpy as np import numpy.linalg as lg import matplotlib.pyplot as plt import scipy.sparse.linalg as sla

URI to the Crazyflie to connect to uri = uri_helper.uri_from_env(default='radio://0/100/2M/E7E7E7E701')

rigid_body_name = 'cf1'

send_full_pose = False

orientation_std_dev = 8.0e-3

class MocapWrapper(Thread): def init(self,scf,uri): Thread.init(self) self.scf = scf self.cf = scf.cf self.command = scf.cf.commander self.uri = uri self.first = 1 self.i = 1 self.j = 1 self.pos_cb = [0,0,0] self.pos_mocap = np.array([0,0,0]) self.vel_mocap = np.array([0,0,0]) self.pos_init = np.array([0,0,0]) self.vel_cb = [0,0,0] self.euler_cb = [0,0,0] self.ngvelcb = [0,0,0] self.thrusts = [] self.thrust = [] self.thrust_current = 0 self.constthrust = 47900.0 self.acc2thr = 4180.0 self.p = 1000000.0 self.gamma = 0 self.k = 0 self.rho = 0.998 self.acc_real = [0,0,0] self.norm_acc = 0 self.acc2thr = [] self.error_thrust = [] self.h_int = [0,0,0] self.send_full_pose = False self.orientation_std_dev = 8.0e-3 self.z_list = []

self.Kp = np.array([2, 2, 2])
self.Kv = np.array([1,1,1])
self.des_acc = np.array([0,0,0])
self.des_vel = np.array([0,0,0])
self.a_des = np.array([0,0,9.81])
rospy.Subscriber("/vrpn_client_node/MCServer/3/pose",PoseStamped,self.pos_callback,queue_size=1)
rospy.Subscriber("/vrpn_client_node/MCServer/3/velocity",TwistStamped,self.vel_callback,queue_size=1)

def wait_for_position_estimator(self): print('Waiting for estimator to find position...')

log_config = LogConfig(name='Kalman Variance', period_in_ms=10)
log_config.add_variable('kalman.varPX', 'FP16')
log_config.add_variable('kalman.varPY', 'FP16')
log_config.add_variable('kalman.varPZ', 'FP16')

var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10

threshold = 0.001

with SyncLogger(self.scf, log_config) as logger:
    for log_entry in logger:
        data = log_entry[1]

        var_x_history.append(data['kalman.varPX'])
        var_x_history.pop(0)
        var_y_history.append(data['kalman.varPY'])
        var_y_history.pop(0)
        var_z_history.append(data['kalman.varPZ'])
        var_z_history.pop(0)

        min_x = min(var_x_history)
        max_x = max(var_x_history)
        min_y = min(var_y_history)
        max_y = max(var_y_history)
        min_z = min(var_z_history)
        max_z = max(var_z_history)

        print("{} {} {}".
            format(max_x - min_x, max_y - min_y, max_z - min_z))

        if (max_x - min_x) < threshold and (
                max_y - min_y) < threshold and (
                max_z - min_z) < threshold:
            break

def _sqrt(self,a):

if a < 0.0:
    return 0.0
return math.sqrt(a)

def send_extpose_quat(self, x, y, z, quat): if send_full_pose: self.cf.extpos.send_extpose(x, y, z, quat.x, quat.y, quat.z, quat.w) else: self.cf.extpos.send_extpos(x, y, z)

def reset_estimator(self): self.cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self.cf.param.set_value('kalman.resetEstimation', '0')

# time.sleep(1)
self.wait_for_position_estimator()

def position_callback(self,timestamp, data, logconf): self.pos_cb[0] = data['kalman.stateX'] self.pos_cb[1] = data['kalman.stateY'] self.pos_cb[2] = data['kalman.stateZ'] self.vel_cb[0] = data['stateEstimate.vx'] self.vel_cb[1] = data['stateEstimate.vy'] self.vel_cb[2] = data['stateEstimate.vz']

def start_position_printing(self): log_conf = LogConfig(name='Position', period_in_ms=10) log_conf.add_variable('kalman.stateX', 'FP16') log_conf.add_variable('kalman.stateY', 'FP16') log_conf.add_variable('kalman.stateZ', 'FP16') log_conf.add_variable('stateEstimate.vx', 'FP16') log_conf.add_variable('stateEstimate.vy', 'FP16') log_conf.add_variable('stateEstimate.vz', 'FP16')

self.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.position_callback)
log_conf.start()

def position_callback_euler(self,imestamp, data, logconf): self.euler_cb[0] = data['stabilizer.roll'] self.euler_cb[1] = data['stabilizer.pitch'] self.euler_cb[2] = data['stabilizer.yaw'] self.acc_real = [data['acc.x']9.81, data['acc.y']9.81, data['acc.z']9.81] self.thrust.append(data['stabilizer.thrust']) self.thrustcurrent = data['stabilizer.thrust'] self.thrusts.append(int(10001 + ((self.const_thrust[0]-10001.0)/9.81)lg.norm(self.acc_real)))

def estimateThrustmodel(self):

model: thrust = acc*acc2thr

self.norm_acc = lg.norm(self.acc_real)
self.gamma = 1/(self.rho + self.norm_acc * self.p * self.norm_acc)
self.k= self.gamma * self.p * self.norm_acc
self.acc2thr_ = self.acc2thr_ + self.k * (self.thrust_current - self.norm_acc * self.acc2thr_)
self.acc2thr.append(self.acc2thr_)
self.p = (1-self.k * self.norm_acc)*self.p / self.rho

def start_position_printing_euler(self): log_conf = LogConfig(name='Stabilizer', period_in_ms=10) log_conf.add_variable('stabilizer.roll', 'FP16') log_conf.add_variable('stabilizer.pitch', 'FP16') log_conf.add_variable('stabilizer.yaw', 'FP16') log_conf.add_variable('stabilizer.thrust','FP16') log_conf.add_variable('acc.x', 'FP16') log_conf.add_variable('acc.y', 'FP16') log_conf.add_variable('acc.z', 'FP16')

self.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.position_callback_euler)
log_conf.start()

def stop_logconf(self): log_conf = LogConfig(name='Position', period_in_ms=10) log_conf.stop()

def adjust_orientation_sensitivity(self): self.cf.param.set_value('locSrv.extQuatStdDev', orientation_std_dev) print('adjust_orientation_senstivity')

def activate_kalman_estimator(self): self.cf.param.set_value('stabilizer.estimator', '2')

# Set the std deviation for the quaternion data pushed into the
# kalman filter. The default value seems to be a bit too low.
self.cf.param.set_value('locSrv.extQuatStdDev', 0.06)
print('activate_kalman_estimate')

def activate_high_level_commander(self): self.cf.param.set_value('commander.enHighLevel', '1') print('activate_high_level_commander')

def activate_mellinger_controller(self): self.cf.param.set_value('stabilizer.controller', '2')

def run_sequence(self): commander = self.cf.high_level_commander

commander.takeoff(0.5, 2.0)
time.sleep(3.0)
commander.land(0.0, 2.0)
time.sleep(2)
commander.stop()

def pos_callback(self,msg): if self.first == 1: self.pos_mocap = np.array([msg.pose.position.x/1000,msg.pose.position.y/1000,msg.pose.position.z/1000]) self.pos_init = self.pos_mocap self.pos_init[2] = 0.4 self.first = 0 if self.i==1: self.pos_mocap = np.array([msg.pose.position.x/1000,msg.pose.position.y/1000,msg.pose.position.z/1000]) self.send_extpose_quat(msg.pose.position.x/1000,msg.pose.position.y/1000,msg.pose.position.z/1000,msg.pose.orientation ) self.i=0

    else :
        self.i=1

def vel_callback(self,msg): if self.j==1: self.vel_mocap = np.array([msg.twist.linear.x,msg.twist.linear.y,msg.twist.linear.z]) self.j=0 else : self.j=1 def thrust_clip(self,thrust_cmd):

thrust clip thrust

if (thrust_cmd > 60000):
    return 50000
elif (thrust_cmd < 10001):
    return  10001
else:
    return thrust_cmd

def vel_control(self): controller1 = PID(1.1,0.002,0.275) controller2 = PID(0.9,0.002,0.275) controller3 = PID(0.9,0.002,0.275)

    t = 0.0
    self.command.send_setpoint(0,0,0,0)
    time.sleep(0.1)
    while t <= 5:#traj1.duration:
        time1 = time.time()

            #PID
            #velocityx = controller1.update(0,self.pos_init[0],self.pos_mocap[0])
            #velocityy = controller2.update(0,self.pos_init[1],self.pos_mocap[1])
            #velocityz = controller3.update(0,self.pos_init[2],self.pos_mocap[2])
        self.des_acc = (self.a_des + np.diag(self.Kv).dot(self.des_vel-self.vel_mocap) + np.diag(self.Kp).dot(self.pos_init-self.pos_mocap))
            #mc.start_linear_motion(velocityx,velocityy,velocityz)
        self.command.send_setpoint(0,0,0,int(self.thrust_clip(self.des_acc[2]*4180)))
        #self.estimateThrustmodel()
        time2 = time.time()
        time3 = time2 -time1
            #print("t=", t)

        if time3 <= 0.01:
            t = t + 0.01
            time.sleep(0.01 - time3)
        else:
            t = t + time3

    #return self.z_list

def plot(self): print('estimate acc2thr') print(self.acc2thr) print('acc') print(self.acc_real) fig = plt.figure() ax1 = fig.addsubplot(511) ax1.plot(self.thrusts) #根据加速度算出来的推力 ax2 = fig.add_subplot(512) ax2.plot(self. thrust)#log出来的推力 ax3 = fig.add_subplot(513) ax3.plot(self.acc_real) #加速度 ax4 = fig.add_subplot(514) ax4.plot(self.acc2thr) #系数 ax5 = fig.add_subplot(515) ax5.plot(self.error_thrust)#推力误差

plt.show()

if name == 'main': cflib.crtp.init_drivers()

Connect to QTM

rospy.init_node('mocap',anonymous=True) with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: cf = scf.cf MW = MocapWrapper(scf,uri)

# accattcon = AccAttController(cf,qtm_wrapper)
MW.adjust_orientation_sensitivity()
MW.activate_kalman_estimator()
#activate_high_level_commander(cf)
# activate_mellinger_controller(cf)
MW.reset_estimator()
# run_sequence(cf, trajectory_id, duration)
MW.start_position_printing()
MW.start_position_printing_euler()
MW.vel_control()
MW.plot()

IMRCLab / crazyswarm2

send_setpoint error #393

model: thrust = acc*acc2thr

thrust clip thrust

Connect to QTM