send_setpoint error

[fangjir]

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.ngvel_cb = [0,0,0]
        self.thrusts_ = []
        self.thrust = []
        self.thrust_current = 0
        self.const_thrust = 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.thrust_current = 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.add_subplot(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()

[knmcguire]

hi! Sorry for the late reply. As you noticed, I had to convert your issue to a discussion as it is not clear if this is a bug on our end or a issue of your own code.

So there are two things:

• It is not clear in your question of what the problem is and what you'd like to solve. What is exactly the issue?
• This is a huge amount of code, so this will be really difficult for us to debug with the limited time we have. Could you please provide a minimal working example if you can