How to use cmdPosition or goTo to position hold?

[minisol0175]

Hello,

I'm trying to use vicon to control 1 crazyflie, but I don't know where the problem is.
Here are the issues.

1. When takeoff, It cannot hold position.

2. I wanted the drone to go to (0,0,0.5), but it is not.

My code is here

---

from pycrazyswarm import Crazyswarm

TAKEOFF_DURATION = 3.0
HOVER_DURATION = 1.0

def main():
swarm = Crazyswarm()
timeHelper = swarm.timeHelper
cf = swarm.allcfs.crazyflies[0]

pos = [0.0, 0.0, 0.5]

timeHelper.sleep(3)

cf.takeoff(targetHeight=0.5, duration=TAKEOFF_DURATION)
timeHelper.sleep(TAKEOFF_DURATION + HOVER_DURATION)
cf.cmdPosition(pos)
timeHelper.sleep(2)
# cf.goTo(pos, 0, 1.0)
# timeHelper.sleep(1.5)
cf.land(targetHeight=0.04, duration=2.0)
timeHelper.sleep(2.0)

if name == "main":
main()

---

The first video used cmdPosition, and the second used goTo.

Initial position is (0,0,0).

object_tracking_type is "libobjecttracker"

cmdPosition.mp4

goTo.mp4

[minisol0175]

This is another case.

I used figure8_scv.py, and this video is the result.

In this case, other commands do not work after takeoff.

20210526_164029.1.mp4

And I want to know what code it flew 49 crazyflies.

Thanks a lot!

[jpreiss]

It looks like something is wrong with your tracking. Takeoff should always go straight up.

cmdPosition is a low-level streaming setpoint mode - see https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/sensor-to-control/commanders_setpoints/. If you use cmdPosition, it should be in a loop sending new setpoints many times per second.

If you want to send one command and then sleep, you should use goTo.

[minisol0175]

Thank you for the reply!

And I checked ros data.

When I run hover_swarm.launch, I can check rostopic /tf and /diagnostics.

rostopic echo /tf

transforms:

header: 
  seq: 0
  stamp: 
    secs: 1622194062
    nsecs: 595357461
  frame_id: "world"
child_frame_id: "cf1"
transform: 
  translation: 
    x: -0.00205390132032
    y: -0.00220347591676
    z: 0.0401952303946
  rotation: 
    x: 0.000798222412731
    y: 0.00280501359966
    z: 0.0452210816476
    w: 0.998972746632

rostopic echo /diagnostics

header:
seq: 116
stamp:
secs: 1622193806
nsecs: 24652815
frame_id: ''
status:

level: 2
name: "joy: Joystick Driver Status"
message: "Joystick not open."
hardware_id: "none"
values: 
  - 
    key: "topic"
    value: "/joy"
  - 
    key: "device"
    value: "/dev/input/js0"
  - 
    key: "device name"
    value: ''
  - 
    key: "dead zone"
    value: "0.05"
  - 
    key: "autorepeat rate (Hz)"
    value: "0"
  - 
    key: "coalesce interval (s)"
    value: "0.001"
  - 
    key: "recent joystick event rate (Hz)"
    value: "0"
  - 
    key: "recent publication rate (Hz)"
    value: "0"
  - 
    key: "subscribers"
    value: "1"
  - 
    key: "default trig val"
    value: "False"
  - 
    key: "sticky buttons"
    value: "False"

And this is tracking part in hover_swarm.launch.

  # tracking
  motion_capture_type: "vicon" # one of none,vicon,optitrack,qualisys,vrpn
  object_tracking_type: "libobjecttracker" # one of motionCapture,libobjecttracker
  send_position_only: True # set to False to send position+orientation; set to True to send position only
  vicon_host_name: "192.168.0.8"
  # optitrack_host_name: "optitrack"
  # qualisys_host_name: "10.0.5.219"
  # qualisys_base_port: 22222
  # vrpn_host_name: "vicon"
  save_point_clouds: ~/pointCloud.ot
  print_latency: False
  write_csvs: False
  force_no_cache: False
  enable_parameters: True
  enable_logging: False

I think crazyflie cannot receive its position data,

transform:
translation:
x: -0.00205390132032
y: -0.00220347591676
z: 0.0401952303946

Which part should I check?

Thanks for your help!

[jpreiss]

If you have the LED ring attachment for your Crazyflie and your hover_swarm.launch has the lines:

ring:
  effect: 16 # 6: double spinner, 7: solid color, 16: packetRate

then the LED ring will be green if the position packets are being received, and red if not. This is the easiest diagnostic tool I know of.

[minisol0175]

Thank you!

I tried it, and it was red...