(not an issue, but a question) How to determine orientation of the crazyflie?

[ugrm]

I wanted to look into the rotation of the crazyflie, more specifically its orientation (what direction it's looking at). The ** multiranger_pointcloud.py** looked like a good file to use.
However, I'm confused as to whether we should use the roll, pitch, yaw values or the rot function down below (in multiranger_pointcloud.py) to determine the orientation of the crazyflie...

` def rot(self, roll, pitch, yaw, origin, point):
cosr = math.cos(math.radians(roll))
cosp = math.cos(math.radians(pitch))
cosy = math.cos(math.radians(yaw))

    sinr = math.sin(math.radians(roll))
    sinp = math.sin(math.radians(pitch))
    siny = math.sin(math.radians(yaw))

    roty = np.array([[cosy, -siny, 0],
                     [siny, cosy, 0],
                     [0, 0,    1]])

    rotp = np.array([[cosp, 0, sinp],
                     [0, 1, 0],
                     [-sinp, 0, cosp]])

    rotr = np.array([[1, 0,   0],
                     [0, cosr, -sinr],
                     [0, sinr,  cosr]])

    rotFirst = np.dot(rotr, rotp)

    rot = np.array(np.dot(rotFirst, roty))

    tmp = np.subtract(point, origin)
    tmp2 = np.dot(rot, tmp)
    
    return np.add(tmp2, origin)`

[tobbeanton]

When you are talking about orientation are you meaning "world" coordinates or body-frame coordinates. If you are not using an external positioning system from where you can get "world" coordinates it is hard to know where you are looking. You could, if you have a map to figure out you position from (SLAM), get that info but that I think is really hard and nothing we have support for.

[ugrm]

I was actually leaning more into body-frame coordinates. To be specific, I wanted to find the relative values of the robot to use in a pose graph. If I remember correctly, the crazyflie used the flowdeck to determine what direction it was moving in. So I thought we could get the relative (x, y, orientation(=yaw)) values with respect to timestep t-1 (assuming we are in timestep t). What values should I be looking at?

In the python file "multiranger_pointcloud.py" in the crazyflie python library, I was looking at

def meas_data(self, timestamp, data, logconf): measurement = { 'roll': data['stabilizer.roll'], 'pitch': data['stabilizer.pitch'], 'yaw': data['stabilizer.yaw'], 'front': data['range.front'], 'back': data['range.back'], 'up': data['range.up'], 'down': data['range.zrange'], 'left': data['range.left'], 'right': data['range.right'] }

for the orientation (=yaw) and

def pos_data(self, timestamp, data, logconf): position = [ data['stateEstimate.x'], data['stateEstimate.y'], data['stateEstimate.z'] ] self.canvas.set_position(position)

for the relative position (x,y).
I drew a pose graph and didn't get a reasonable graph. Is there another function in "multiranger_pointcloud.py" I should be looking at?

[ugrm]

Right now, I'm finding the relative position and orientation by subtracting values at timestep t-1 from values at timestep t. I think there are much more direct ways to do it though,,,

[ugrm]

It just turns out I had to use convert from a drone's frame into the global frame instead of getting rudimentary values.

[PioBeat]

Hi @ugrm I am also facing the same task right now.
Could you explain how you managed to solve this? Specifically, the changes to the multiranger_pointcloud.py that were necessary?
Thanks!