Explanations about the cmdFullState topic #507

GusBon · 2024-05-27T09:18:47Z

GusBon
May 27, 2024

Hi,
I am currently trying to control drones by controlling either their position in space and their attitude. I know that a quadcopter only has 4 degree of freedom, so the attitude and position in space are heavily related.
I have found the cmdFullState topic and I have some questions about it.

crazyswarm2/crazyflie_py/crazyflie_py/crazyflie.py

Lines 559 to 602 in 97ed0a9

    
               def cmdFullState(self, pos, vel, acc, yaw, omega): 
        
                   """ 
        
                   Send a streaming full-state controller setpoint command. 
        
                   The full-state setpoint is most useful for aggressive maneuvers where 
        
                   feedforward inputs for acceleration and angular velocity are critical 
        
                   to obtaining good tracking performance. Full-state setpoints can be 
        
                   obtained from any trajectory parameterization that is at least three 
        
                   times differentiable, e.g. piecewise polynomials. 
        
                   Sending a streaming setpoint of any type will force a change from 
        
                   high-level to low-level command mode. Currently, there is no mechanism 
        
                   to change back, but it is a high-priority feature to implement. 
        
                   This means it is not possible to use e.g. :meth:`land()` or 
        
                   :meth:`goTo()` after a streaming setpoint has been sent. 
        
                   Args: 
        
                       pos (array-like of float[3]): Position. Meters. 
        
                       vel (array-like of float[3]): Velocity. Meters / second. 
        
                       acc (array-like of float[3]): Acceleration. Meters / second^2. 
        
                       yaw (float): Yaw angle. Radians. 
        
                       omega (array-like of float[3]): Angular velocity in body frame. 
        
                           Radians / sec. 
        
                   """ 
        
                   self.cmdFullStateMsg.header.stamp = self.node.get_clock().now().to_msg() 
        
                   self.cmdFullStateMsg.pose.position.x = pos[0] 
        
                   self.cmdFullStateMsg.pose.position.y = pos[1] 
        
                   self.cmdFullStateMsg.pose.position.z = pos[2] 
        
                   self.cmdFullStateMsg.twist.linear.x = vel[0] 
        
                   self.cmdFullStateMsg.twist.linear.y = vel[1] 
        
                   self.cmdFullStateMsg.twist.linear.z = vel[2] 
        
                   self.cmdFullStateMsg.acc.x = acc[0] 
        
                   self.cmdFullStateMsg.acc.y = acc[1] 
        
                   self.cmdFullStateMsg.acc.z = acc[2] 
        
                   q = rowan.from_euler(0, 0, yaw) 
        
                   self.cmdFullStateMsg.pose.orientation.w = q[0] 
        
                   self.cmdFullStateMsg.pose.orientation.x = q[1] 
        
                   self.cmdFullStateMsg.pose.orientation.y = q[2] 
        
                   self.cmdFullStateMsg.pose.orientation.z = q[3] 
        
                   self.cmdFullStateMsg.twist.angular.x = omega[0] 
        
                   self.cmdFullStateMsg.twist.angular.y = omega[1] 
        
                   self.cmdFullStateMsg.twist.angular.z = omega[2] 
        
                   self.cmdFullStatePublisher.publish(self.cmdFullStateMsg)

Firstly, I want to know if I have well understood the topic. With this topic you can send a command with a desired pose, a desired linear velocity, a desired yaw and a desired angular velocity ?
Secondly, by taking in account that the answer to my first point is true, how is it possible for a quadcopter which has 4 DoF to send a desired pose and linear velocity in the same time ? Moreover how can you set a desired pose and angular velocity in the same time ? The angular part and the linear part need to be coherent together to not be contradictory.

So I want to know how the things works under the hood of this topic, to understand better if it is a useful topic for my use case and how to use it well.
Thanks a lot in advance !

magixn · 2024-11-06T14:09:35Z

magixn
Nov 6, 2024

I think it solves inside like the polynomial trajectory a to get all desired parameters satisfied. I wonder if I just want to change its current linear velocity, any formulas to convert it.

2 replies

knmcguire Nov 12, 2024
Maintainer

So, are you talking about body fixed velocity? Perhaps best just to start a new discussion and explain on a high level what you'd want to achieve. There might be other services/topics that will be better suited for this.

magixn Nov 12, 2024

Yes, I just start a new discussion #588 about usage issue on cmdFullState. I find this function is more for planning as it is useful for evaluating the trajectory.

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Explanations about the cmdFullState topic #507

{{title}}

Replies: 1 comment 2 replies

{{title}}

{{title}}

{{title}}

{{editor}}'s edit

{{editor}}'s edit

Select a reply

Explanations about the cmdFullState topic #507

GusBon May 27, 2024

Replies: 1 comment · 2 replies

magixn Nov 6, 2024

knmcguire Nov 12, 2024 Maintainer

magixn Nov 12, 2024

GusBon
May 27, 2024

Replies: 1 comment 2 replies

magixn
Nov 6, 2024

knmcguire Nov 12, 2024
Maintainer