//
// Created by SJQ on 2024/4/26.
//

#ifndef BASIC_FRAMEWORK_ROBOTICS_H
#define BASIC_FRAMEWORK_ROBOTICS_H
#include "user_lib.h"

// rotation matrix(R)   RPY([yaw,pitch,roll])     quaternion([q0,q1,q2,q3])
void r2rpy(const mat* R,float rpy[3]);
void rpy2r(float rpy[3],mat* R);

void r2quat(const mat*R,float q[4]);
void quat2r(float q[4],mat* R);

void quat2rpy(const float q[4],float rpy[3]);
void rpy2quat(const float rpy[3],float q[4]);



#endif //BASIC_FRAMEWORK_ROBOTICS_H