Regarding cmd_vel_legacy command

[Sapio-S]

Hi!

I found no implementation of the cmd_vel_legacy command in crazyswarm2, so I implemented one in crazyflie_py/crazyflie.py based on crazyswarm1:

    def cmdVel(self, roll, pitch, yawrate, thrust):
    #     """Sends a streaming command of the "easy mode" manual control inputs.

    #     Args:
    #         roll (float): Roll angle. Degrees. Positive values == roll right.
    #         pitch (float): Pitch angle. Degrees. Positive values == pitch
    #             forward/down.
    #         yawrate (float): Yaw angular velocity. Degrees / second. Positive
    #             values == turn counterclockwise.
    #         thrust (float): Thrust magnitude. Non-meaningful units in [0, 2^16),
    #             where the maximum value corresponds to maximum thrust.
    #     """
        msg = Twist()
        msg.linear.x = pitch
        msg.linear.y = roll
        msg.angular.z = yawrate
        msg.linear.z = thrust
        self.cmdVelPublisher.publish(msg)

However, I noticed that in crazyflie_server.py:

     def _cmd_vel_legacy_changed(self, msg, uri=""): 
         """ 
         Topic update callback to control the attitude and thrust 
             of the crazyflie with teleop 
         """ 
         roll = msg.linear.y 
         pitch = -msg.linear.x 
         yawrate = msg.angular.z 
         thrust = int(min(max(msg.linear.z, 0, 0), 60000)) 
         self.swarm._cfs[uri].cf.commander.send_setpoint( 
             roll, pitch, yawrate, thrust) 

In the above function, the pitch term is opposite to the msg value. This makes me wonder if I implemented the pitch term correctly in crazyflie.py. Why is there a - sign in the function?

Also, I want to send cmd_vel commands after the drone stably takes off. In other words, I want to change from high-level control to low-level control mid-way. However, my crazyflie seems to ignore low-level commands and drops directly after taking off. How should I fix this? Or, is it possible to use low-level command directly after high-level commands? My code is as follows.

    cf.takeoff(targetHeight=0.5, duration=2.5)
    timeHelper.sleep(5.0)
    for _ in range(100):
        cf.cmdVel(0., 0., 0., 0.63 * 2**16)
        timeHelper.sleep(0.01)

Thanks a lot!

[Eric-Ewing]

I'm going to skip to the second part. You can switch directly from high level commands to low level commands. I'm going to suggest you test your setup with cmdPos first, as I know that works and is easier to get working than velocity based commands.

I recommend using timeHelper.sleepForRate(Hz) instead of the normal sleep(), the advantage to sleepForRate is that it keeps the rate as consistent as possible, even if you loop contains other code that needs to run that takes non-zero time. Try something like this (I can't test this at the moment, so there might be some bugs, but it should be close...)

cf.takeoff(0.5, 0.25)
timeHelper.sleep(0.5)

# Make a circle, assumes crazyflie starts at x=0, y=0
Hz = 20
time_scaling = 1/4
fx = lambda t: np.sin(t*time_scaling)
fy = lambda t: np.sin(t*time_scaling)

# Make a list of evenly spaced time values from 0 to 8pi (duration of circle)
timesteps = np.arange(0, 2*np.pi/time_scaling, 1/Hz)
for timestep in timesteps:
    x = fx(timestep)
    y = fy(timestep)
    z = 0.5
    yaw = 0
    cf.cmdPos((x, y, z), yaw)
    timeHelper.sleepForRate(Hz)

cf.notifySetpointsStop()
cf.land(0.0, 2.0)

If you can get this to work, you can verify that low level commands do in fact work and the issue is with your usage/implementation of cmdVel.

[Sapio-S]

Hi!

Thank you for your suggestions. Unfortunately, your proposed code could not work. I further tried crazyflie_examples/cmd_full_state.py and found it not working as well. When executing the cmdPosition() command, the drone simply stayed where it was. After (maybe not actually) running the low-level commands, it was able to execute high-level commands such ascf.notifySetpointsStop() and cf.land(), though.

I also tried running cf.cmdVel() from the beginning without high-level commands, and it worked, the drone took off. But when I ran cf.cmdVel() after cf.takeoff(), the drone just dropped to the ground immediately, although I was sending it a series of commands that should be able to keep it hovering in the air.