diff --git a/src/rl_sar/library/loop/loop.hpp b/src/rl_sar/library/core/loop/loop.hpp similarity index 100% rename from src/rl_sar/library/loop/loop.hpp rename to src/rl_sar/library/core/loop/loop.hpp diff --git a/src/rl_sar/library/matplotlibcpp/matplotlibcpp.h b/src/rl_sar/library/core/matplotlibcpp/matplotlibcpp.h similarity index 100% rename from src/rl_sar/library/matplotlibcpp/matplotlibcpp.h rename to src/rl_sar/library/core/matplotlibcpp/matplotlibcpp.h diff --git a/src/rl_sar/library/observation_buffer/observation_buffer.cpp b/src/rl_sar/library/core/observation_buffer/observation_buffer.cpp similarity index 100% rename from src/rl_sar/library/observation_buffer/observation_buffer.cpp rename to src/rl_sar/library/core/observation_buffer/observation_buffer.cpp diff --git a/src/rl_sar/library/observation_buffer/observation_buffer.hpp b/src/rl_sar/library/core/observation_buffer/observation_buffer.hpp similarity index 100% rename from src/rl_sar/library/observation_buffer/observation_buffer.hpp rename to src/rl_sar/library/core/observation_buffer/observation_buffer.hpp diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/core/rl_sdk/rl_sdk.cpp similarity index 92% rename from src/rl_sar/library/rl_sdk/rl_sdk.cpp rename to src/rl_sar/library/core/rl_sdk/rl_sdk.cpp index 189879e..4395cf8 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.cpp +++ b/src/rl_sar/library/core/rl_sdk/rl_sdk.cpp @@ -458,45 +458,14 @@ std::vector ReadVectorFromYaml(const YAML::Node &node) return values; } -template -std::vector ReadVectorFromYaml(const YAML::Node &node, const std::string &framework, const int &rows, const int &cols) +void RL::ReadYaml(std::string robot_path) { - std::vector values; - for (const auto &val : node) - { - values.push_back(val.as()); - } - - if (framework == "isaacsim") - { - std::vector transposed_values(cols * rows); - for (int r = 0; r < rows; ++r) - { - for (int c = 0; c < cols; ++c) - { - transposed_values[c * rows + r] = values[r * cols + c]; - } - } - return transposed_values; - } - else if (framework == "isaacgym") - { - return values; - } - else - { - throw std::invalid_argument("Unsupported framework: " + framework); - } -} - -void RL::ReadYaml(std::string robot_name) -{ - // The config file is located at "rl_sar/src/rl_sar/models//config.yaml" - std::string config_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_name + "/config.yaml"; + // The config file is located at "rl_sar/src/rl_sar/models/params.model_name = config["model_name"].as(); this->params.framework = config["framework"].as(); - int rows = config["rows"].as(); - int cols = config["cols"].as(); this->params.dt = config["dt"].as(); this->params.decimation = config["decimation"].as(); this->params.num_observations = config["num_observations"].as(); @@ -528,8 +495,8 @@ void RL::ReadYaml(std::string robot_name) } else { - this->params.clip_actions_upper = torch::tensor(ReadVectorFromYaml(config["clip_actions_upper"], this->params.framework, rows, cols)).view({1, -1}); - this->params.clip_actions_lower = torch::tensor(ReadVectorFromYaml(config["clip_actions_lower"], this->params.framework, rows, cols)).view({1, -1}); + this->params.clip_actions_upper = torch::tensor(ReadVectorFromYaml(config["clip_actions_upper"])).view({1, -1}); + this->params.clip_actions_lower = torch::tensor(ReadVectorFromYaml(config["clip_actions_lower"])).view({1, -1}); } this->params.action_scale = config["action_scale"].as(); this->params.hip_scale_reduction = config["hip_scale_reduction"].as(); @@ -543,13 +510,15 @@ void RL::ReadYaml(std::string robot_name) this->params.dof_vel_scale = config["dof_vel_scale"].as(); this->params.commands_scale = torch::tensor(ReadVectorFromYaml(config["commands_scale"])).view({1, -1}); // this->params.commands_scale = torch::tensor({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale}); - this->params.rl_kp = torch::tensor(ReadVectorFromYaml(config["rl_kp"], this->params.framework, rows, cols)).view({1, -1}); - this->params.rl_kd = torch::tensor(ReadVectorFromYaml(config["rl_kd"], this->params.framework, rows, cols)).view({1, -1}); - this->params.fixed_kp = torch::tensor(ReadVectorFromYaml(config["fixed_kp"], this->params.framework, rows, cols)).view({1, -1}); - this->params.fixed_kd = torch::tensor(ReadVectorFromYaml(config["fixed_kd"], this->params.framework, rows, cols)).view({1, -1}); - this->params.torque_limits = torch::tensor(ReadVectorFromYaml(config["torque_limits"], this->params.framework, rows, cols)).view({1, -1}); - this->params.default_dof_pos = torch::tensor(ReadVectorFromYaml(config["default_dof_pos"], this->params.framework, rows, cols)).view({1, -1}); - this->params.joint_controller_names = ReadVectorFromYaml(config["joint_controller_names"], this->params.framework, rows, cols); + this->params.rl_kp = torch::tensor(ReadVectorFromYaml(config["rl_kp"])).view({1, -1}); + this->params.rl_kd = torch::tensor(ReadVectorFromYaml(config["rl_kd"])).view({1, -1}); + this->params.fixed_kp = torch::tensor(ReadVectorFromYaml(config["fixed_kp"])).view({1, -1}); + this->params.fixed_kd = torch::tensor(ReadVectorFromYaml(config["fixed_kd"])).view({1, -1}); + this->params.torque_limits = torch::tensor(ReadVectorFromYaml(config["torque_limits"])).view({1, -1}); + this->params.default_dof_pos = torch::tensor(ReadVectorFromYaml(config["default_dof_pos"])).view({1, -1}); + this->params.joint_controller_names = ReadVectorFromYaml(config["joint_controller_names"]); + this->params.command_mapping = ReadVectorFromYaml(config["command_mapping"]); + this->params.state_mapping = ReadVectorFromYaml(config["state_mapping"]); } void RL::CSVInit(std::string robot_name) diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/core/rl_sdk/rl_sdk.hpp similarity index 96% rename from src/rl_sar/library/rl_sdk/rl_sdk.hpp rename to src/rl_sar/library/core/rl_sdk/rl_sdk.hpp index a948d66..5dabf1d 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.hpp +++ b/src/rl_sar/library/core/rl_sdk/rl_sdk.hpp @@ -108,6 +108,8 @@ struct ModelParams torch::Tensor commands_scale; torch::Tensor default_dof_pos; std::vector joint_controller_names; + std::vector command_mapping; + std::vector state_mapping; }; struct Observations @@ -152,7 +154,7 @@ public: torch::Tensor QuatRotateInverse(torch::Tensor q, torch::Tensor v, const std::string &framework); // yaml params - void ReadYaml(std::string robot_name); + void ReadYaml(std::string robot_path); // csv logger std::string csv_filename; @@ -164,7 +166,7 @@ public: void KeyboardInterface(); // others - std::string robot_name; + std::string robot_name, config_name; STATE running_state = STATE_RL_RUNNING; // default running_state set to STATE_RL_RUNNING bool simulation_running = false; diff --git a/src/rl_sar/library/thirdparty/l4w4_sdk/comm.h b/src/rl_sar/library/thirdparty/l4w4_sdk/comm.h new file mode 100644 index 0000000..af71d61 --- /dev/null +++ b/src/rl_sar/library/thirdparty/l4w4_sdk/comm.h @@ -0,0 +1,60 @@ +#ifndef L4W4_SDK_COMM_H +#define L4W4_SDK_COMM_H + +#include + +typedef struct +{ + float x; + float y; + float z; +} Vector3; + +typedef struct +{ + float quaternion[4]; // quaternion, normalized, (w,x,y,z) + float gyroscope[3]; // angular velocity (unit: rad/s) + float accelerometer[3]; // m/(s2) + float rpy[3]; // euler angle(unit: rad) + int8_t temperature; +} IMU; // when under accelerated motion, the attitude of the robot calculated by IMU will drift. + +typedef struct +{ + uint8_t mode; // motor working mode + float q; // current angle (unit: radian) + float dq; // current velocity (unit: radian/second) + float ddq; // current acc (unit: radian/second*second) + float tauEst; // current estimated output torque (unit: N.m) + float q_raw; // current angle (unit: radian) + float dq_raw; // current velocity (unit: radian/second) + float ddq_raw; + int8_t temperature; // current temperature (temperature conduction is slow that leads to lag) + uint32_t reserve[2]; +} MotorState; // motor feedback + +typedef struct +{ + uint8_t mode; // desired working mode + float q; // desired angle (unit: radian) + float dq; // desired velocity (unit: radian/second) + float tau; // desired output torque (unit: N.m) + float Kp; // desired position stiffness (unit: N.m/rad ) + float Kd; // desired velocity stiffness (unit: N.m/(rad/s) ) + uint32_t reserve[3]; +} MotorCmd; // motor control + +typedef struct +{ + IMU imu; + MotorState motorState[20]; + uint8_t wirelessRemote[40]; // wireless commands +} LowState; // low level feedback + +typedef struct +{ + MotorCmd motorCmd[20]; + uint8_t wirelessRemote[40]; +} LowCmd; // low level control + +#endif // L4W4_SDK_COMM_H diff --git a/src/rl_sar/library/thirdparty/l4w4_sdk/joystick.h b/src/rl_sar/library/thirdparty/l4w4_sdk/joystick.h new file mode 100644 index 0000000..2b551b1 --- /dev/null +++ b/src/rl_sar/library/thirdparty/l4w4_sdk/joystick.h @@ -0,0 +1,41 @@ +#ifndef L4W4_SDK_JOYSTICK_H +#define L4W4_SDK_JOYSTICK_H + +#include +// 16b +typedef union { + struct { + uint8_t R1 :1; + uint8_t L1 :1; + uint8_t start :1; + uint8_t select :1; + uint8_t R2 :1; + uint8_t L2 :1; + uint8_t F1 :1; + uint8_t F2 :1; + uint8_t A :1; + uint8_t B :1; + uint8_t X :1; + uint8_t Y :1; + uint8_t up :1; + uint8_t right :1; + uint8_t down :1; + uint8_t left :1; + } components; + uint16_t value; +} xKeySwitchUnion; + +// 40 Byte (now used 24B) +typedef struct { + uint8_t head[2]; + xKeySwitchUnion btn; + float lx; + float rx; + float ry; + float L2; + float ly; + + uint8_t idle[16]; +} xRockerBtnDataStruct; + +#endif // L4W4_SDK_JOYSTICK_H \ No newline at end of file diff --git a/src/rl_sar/library/thirdparty/l4w4_sdk/l4w4_sdk.hpp b/src/rl_sar/library/thirdparty/l4w4_sdk/l4w4_sdk.hpp new file mode 100644 index 0000000..5c339e3 --- /dev/null +++ b/src/rl_sar/library/thirdparty/l4w4_sdk/l4w4_sdk.hpp @@ -0,0 +1,534 @@ +#ifndef L4W4_SDK_HPP +#define L4W4_SDK_HPP + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "joystick.h" +#include "comm.h" + +class L4W4SDK +{ +private: + float tmp_time_from_mcu = -1; + int show_rc = 0; + int ctr = 0; + float t0 = -1; + int n_cpu = 0; + int n_run = 0; + + float read_float(unsigned char *buff, int *idx); + float read_from1byte(unsigned char *buff, int *idx, float v_min, float v_max); + float read_from2bytes(unsigned char *buff, int *idx, float v_min, float v_max); + void write_into2bytes(float value, unsigned char *buff, int *idx, float v_min, float v_max); + float read_byte(unsigned char *buff, int *idx); + float Vector3_Dot(Vector3 v1, Vector3 v2); + Vector3 Vector3_Cross(Vector3 u, Vector3 v); + float Sign(float value); + float Vector3_invSqrt(Vector3 v); + Vector3 Vector3_Direction(Vector3 v); + float Clamp(float value, float min, float max); + float Angle_vA_2_vB(Vector3 vA, Vector3 vB, Vector3 axle); + void Vector3_Normalize(Vector3 *v); + Vector3 Quaternion_Transform(float vx, float vy, float vz, float qx, float qy, float qz, float qw); +public: + L4W4SDK() {}; + ~L4W4SDK() {}; + int client_socket; + struct sockaddr_in server_addr; + int recv_len = 0; + int ex_send_recv = -1; + unsigned char sent_buff[256]; + unsigned char recv_buff[512]; + void InitUDP(); + void AnalyzeUDP(unsigned char *recv_buff, LowState &lowState); + void SendUDP(LowCmd &lowCmd); + void PrintMCU(int running_state); + void InitCmdData(LowCmd& cmd); +}; + +void L4W4SDK::InitCmdData(LowCmd& cmd) +{ + for (int i = 0; i < 20; ++i) { + cmd.motorCmd[i].mode = 0; + cmd.motorCmd[i].q = 0.0; + cmd.motorCmd[i].dq = 0.0; + cmd.motorCmd[i].tau = 0.0; + cmd.motorCmd[i].Kp = 0.0; + cmd.motorCmd[i].Kd = 0.0; + } + memset(cmd.wirelessRemote, 0, sizeof(cmd.wirelessRemote)); +} + +void L4W4SDK::PrintMCU(int running_state) +{ + float t1 = tmp_time_from_mcu; + if(t1 < t0) + t0 = t1; + + float ms = (t1 - t0) * 1000.0; + n_cpu++; + n_run++; + + if(ms > 1000) + { + std::cout<<"mcu time = "<< tmp_time_from_mcu<<", n_cpu = "<= 0) + { + std::cout << "udp bind success (" << ret << ")" <>8) & 0xff; + (*idx)++; + buff[*idx] = v16 & 0xff; + (*idx)++; +} + +float L4W4SDK::read_byte(unsigned char *buff, int *idx) +{ + float v = (float)buff[*idx]; + (*idx)++; + return v; +} + +float L4W4SDK::Vector3_Dot(Vector3 v1, Vector3 v2) +{ + return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z; +} + +Vector3 L4W4SDK::Vector3_Cross(Vector3 u, Vector3 v) +{ + Vector3 vn; + vn.x = u.y * v.z - u.z * v.y; + vn.y = u.z * v.x - u.x * v.z; + vn.z = u.x * v.y - u.y * v.x; + return vn; +} + +float L4W4SDK::Sign(float value) +{ + if(value >= 0) + return 1; + else + return -1; +} + +float L4W4SDK::Vector3_invSqrt(Vector3 v) +{ + return 1.0 / sqrtf(v.x * v.x + v.y * v.y + v.z * v.z); +} + +Vector3 L4W4SDK::Vector3_Direction(Vector3 v) +{ + float invSqrt = Vector3_invSqrt(v); + Vector3 dv = v; + dv.x *= invSqrt; + dv.y *= invSqrt; + dv.z *= invSqrt; + + return dv; +} + +float L4W4SDK::Clamp(float value, float min, float max) +{ + if(min > max) + { + float tmp = min; + min = max; + max = tmp; + } + + if(value < min) return min; + else if(value > max) return max; + else return value; +} + +float L4W4SDK::Angle_vA_2_vB(Vector3 vA, Vector3 vB, Vector3 axle) +{ + Vector3 vA_dir = Vector3_Direction(vA); + Vector3 vB_dir = Vector3_Direction(vB); + Vector3 axle_dir = Vector3_Direction(axle); + + float sinA = Clamp( Vector3_Dot(axle_dir, Vector3_Cross(vA_dir, vB_dir) ), -1, 1); + float angle_raw = asinf(sinA); + + float angle = Vector3_Dot(vA_dir, vB_dir) > 0 ? angle_raw : Sign(angle_raw) * M_PI - angle_raw; + + return angle; +} + +void L4W4SDK::Vector3_Normalize(Vector3 *v) +{ + float invSqrt = Vector3_invSqrt(*v); + v->x *= invSqrt; + v->y *= invSqrt; + v->z *= invSqrt; +} +Vector3 L4W4SDK::Quaternion_Transform(float vx, float vy, float vz, float qx, float qy, float qz, float qw) +{ + float dot_u_v = qx * vx + qy * vy + qz * vz; + float dot_u_u = qx * qx + qy * qy + qz * qz; + float x_of_uXv = qy * vz - qz * vy; + float y_of_uXv = qz * vx - qx * vz; + float z_of_uXv = qx * vy - qy * vx; + float k1 = 2.0 * dot_u_v; + float k2 = qw * qw - dot_u_u; + float k3 = 2.0 * qw; + + Vector3 vout; + vout.x = k1 * qx + k2 * vx + k3 * x_of_uXv; + vout.y = k1 * qy + k2 * vy + k3 * y_of_uXv; + vout.z = k1 * qz + k2 * vz + k3 * z_of_uXv; + return vout; +} + + +void L4W4SDK::AnalyzeUDP(unsigned char *recv_buff, LowState &lowState) +{ + int idx = 2; + tmp_time_from_mcu = read_float(recv_buff, &idx); + float BatteryVoltage = read_from1byte(recv_buff, &idx, 20, 60); + float MCUTemperature = read_byte(recv_buff, &idx); + + float acc_lx = read_float(recv_buff, &idx); + float acc_ly = read_float(recv_buff, &idx); + float acc_lz = read_float(recv_buff, &idx); + float omega_lx = read_float(recv_buff, &idx); + float omega_ly = read_float(recv_buff, &idx); + float omega_lz = read_float(recv_buff, &idx); + float orientation_x = read_float(recv_buff, &idx); + float orientation_y = read_float(recv_buff, &idx); + float orientation_z = read_float(recv_buff, &idx); + float orientation_w = read_float(recv_buff, &idx); + + + + Vector3 robot_up_w = Quaternion_Transform(0, 1, 0, orientation_x, orientation_y, orientation_z, orientation_w); + Vector3 current_trunk_front = Quaternion_Transform(1, 0, 0, orientation_x, orientation_y, orientation_z, orientation_w); + Vector3 current_trunk_right = Quaternion_Transform(0, 0, 1, orientation_x, orientation_y, orientation_z, orientation_w); + Vector3 trunk_hori_front = current_trunk_front; + trunk_hori_front.y = 0; + Vector3_Normalize(&trunk_hori_front); + Vector3 trunk_hori_right = Vector3_Cross(trunk_hori_front, {0,1,0}); + float r2d = 180.0f / M_PI; + + + float robot_yaw_deg = Angle_vA_2_vB({1,0,0}, trunk_hori_front, {0,1,0}) * r2d; + float robot_pitch_deg = Angle_vA_2_vB(trunk_hori_front, current_trunk_front, trunk_hori_right) * r2d; + float robot_roll_deg = Angle_vA_2_vB(trunk_hori_right, current_trunk_right, current_trunk_front) * r2d; + + + read_byte(recv_buff, &idx); + read_byte(recv_buff, &idx); + + xRockerBtnDataStruct *rockerBtn = (xRockerBtnDataStruct*)(&(lowState.wirelessRemote)); + + unsigned char key1 = recv_buff[idx]; idx++; + rockerBtn->btn.components.R1 = (key1 & 0x80) >> 7; + rockerBtn->btn.components.L1 = (key1 & 0x40) >> 6; + rockerBtn->btn.components.start = (key1 & 0x20) >> 5; + rockerBtn->btn.components.select = (key1 & 0x10) >> 4; + rockerBtn->btn.components.R2 = (key1 & 0x08) >> 3; + rockerBtn->btn.components.L2 = (key1 & 0x04) >> 2; + rockerBtn->btn.components.F1 = (key1 & 0x02) >> 1; + rockerBtn->btn.components.F2 = (key1 & 0x01) >> 0; + + unsigned char key2 = recv_buff[idx]; idx++; + rockerBtn->btn.components.A = (key2 & 0x80) >> 7; + rockerBtn->btn.components.B = (key2 & 0x40) >> 6; + rockerBtn->btn.components.X = (key2 & 0x20) >> 5; + rockerBtn->btn.components.Y = (key2 & 0x10) >> 4; + rockerBtn->btn.components.up = (key2 & 0x08) >> 3; + rockerBtn->btn.components.right = (key2 & 0x04) >> 2; + rockerBtn->btn.components.down = (key2 & 0x02) >> 1; + rockerBtn->btn.components.left = (key2 & 0x01) >> 0; + + + rockerBtn->lx = read_float(recv_buff, &idx); + rockerBtn->rx = read_float(recv_buff, &idx); + rockerBtn->ly = read_float(recv_buff, &idx); + rockerBtn->L2 = read_float(recv_buff, &idx); + rockerBtn->ry = read_float(recv_buff, &idx); + + if(rockerBtn->btn.components.F1) + show_rc = 1; + else + show_rc = 0; + + idx+=4*4; + + float motor_leg1_j0 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI); + float motor_leg1_j0_dot = read_from2bytes(recv_buff, &idx, -33, 33); + float motor_leg1_j1 = read_from2bytes(recv_buff, &idx, -1.3*M_PI, 0.7*M_PI); + float motor_leg1_j1_dot = read_from2bytes(recv_buff, &idx, -33, 33); + float motor_leg1_j2 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI); + float motor_leg1_j2_dot = read_from2bytes(recv_buff, &idx, -33, 33); + + float motor_leg2_j0 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI); + float motor_leg2_j0_dot = read_from2bytes(recv_buff, &idx, -33, 33); + float motor_leg2_j1 = read_from2bytes(recv_buff, &idx, -1.3*M_PI, 0.7*M_PI); + float motor_leg2_j1_dot = read_from2bytes(recv_buff, &idx, -33, 33); + float motor_leg2_j2 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI); + float motor_leg2_j2_dot = read_from2bytes(recv_buff, &idx, -33, 33); + + float motor_leg3_j0 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI); + float motor_leg3_j0_dot = read_from2bytes(recv_buff, &idx, -33, 33); + float motor_leg3_j1 = read_from2bytes(recv_buff, &idx, -1.3*M_PI, 0.7*M_PI); + float motor_leg3_j1_dot = read_from2bytes(recv_buff, &idx, -33, 33); + float motor_leg3_j2 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI); + float motor_leg3_j2_dot = read_from2bytes(recv_buff, &idx, -33, 33); + + float motor_leg4_j0 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI); + float motor_leg4_j0_dot = read_from2bytes(recv_buff, &idx, -33, 33); + float motor_leg4_j1 = read_from2bytes(recv_buff, &idx, -1.3*M_PI, 0.7*M_PI); + float motor_leg4_j1_dot = read_from2bytes(recv_buff, &idx, -33, 33); + float motor_leg4_j2 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI); + float motor_leg4_j2_dot = read_from2bytes(recv_buff, &idx, -33, 33); + + float motor_leg1_j3 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI); + float motor_leg1_j3_dot = read_from2bytes(recv_buff, &idx, -200, 200); + float motor_leg2_j3 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI); + float motor_leg2_j3_dot = read_from2bytes(recv_buff, &idx, -200, 200); + float motor_leg3_j3 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI); + float motor_leg3_j3_dot = read_from2bytes(recv_buff, &idx, -200, 200); + float motor_leg4_j3 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI); + float motor_leg4_j3_dot = read_from2bytes(recv_buff, &idx, -200, 200); + + lowState.imu.quaternion[0] = orientation_w; + lowState.imu.quaternion[1] = orientation_x; + lowState.imu.quaternion[2] = -orientation_z; + lowState.imu.quaternion[3] = orientation_y; + + lowState.imu.gyroscope[0] = omega_lx; + lowState.imu.gyroscope[1] = -omega_lz; + lowState.imu.gyroscope[2] = omega_ly; + lowState.imu.accelerometer[0] = acc_lx; + lowState.imu.accelerometer[1] = -acc_lz; + lowState.imu.accelerometer[2] = acc_ly; + + // leg4 leg1 leg3 leg2 + // hip thigh shank wheel + + lowState.motorState[0].q = lowState.motorState[0].q_raw = motor_leg4_j0; + lowState.motorState[0].dq = lowState.motorState[0].dq_raw = motor_leg4_j0_dot; + lowState.motorState[0].ddq = lowState.motorState[0].ddq_raw = 0; + lowState.motorState[1].q = lowState.motorState[1].q_raw = -motor_leg4_j1; + lowState.motorState[1].dq = lowState.motorState[1].dq_raw = -motor_leg4_j1_dot; + lowState.motorState[1].ddq = lowState.motorState[1].ddq_raw = 0; + lowState.motorState[2].q = lowState.motorState[2].q_raw = -motor_leg4_j2; + lowState.motorState[2].dq = lowState.motorState[2].dq_raw = -motor_leg4_j2_dot; + lowState.motorState[2].ddq = lowState.motorState[2].ddq_raw = 0; + lowState.motorState[3].q = lowState.motorState[3].q_raw = -motor_leg4_j3; + lowState.motorState[3].dq = lowState.motorState[3].dq_raw = -motor_leg4_j3_dot; + lowState.motorState[3].ddq = lowState.motorState[3].ddq_raw = 0; + + lowState.motorState[4].q = lowState.motorState[4].q_raw = motor_leg1_j0; + lowState.motorState[4].dq = lowState.motorState[4].dq_raw = motor_leg1_j0_dot; + lowState.motorState[4].ddq = lowState.motorState[4].ddq_raw = 0; + lowState.motorState[5].q = lowState.motorState[5].q_raw = -motor_leg1_j1; + lowState.motorState[5].dq = lowState.motorState[5].dq_raw = -motor_leg1_j1_dot; + lowState.motorState[5].ddq = lowState.motorState[5].ddq_raw = 0; + lowState.motorState[6].q = lowState.motorState[6].q_raw = -motor_leg1_j2; + lowState.motorState[6].dq = lowState.motorState[6].dq_raw = -motor_leg1_j2_dot; + lowState.motorState[6].ddq = lowState.motorState[6].ddq_raw = 0; + lowState.motorState[7].q = lowState.motorState[7].q_raw = -motor_leg1_j3; + lowState.motorState[7].dq = lowState.motorState[7].dq_raw = -motor_leg1_j3_dot; + lowState.motorState[7].ddq = lowState.motorState[7].ddq_raw = 0; + + lowState.motorState[8].q = lowState.motorState[8].q_raw = motor_leg3_j0; + lowState.motorState[8].dq = lowState.motorState[8].dq_raw = motor_leg3_j0_dot; + lowState.motorState[8].ddq = lowState.motorState[8].ddq_raw = 0; + lowState.motorState[9].q = lowState.motorState[9].q_raw = -motor_leg3_j1; + lowState.motorState[9].dq = lowState.motorState[9].dq_raw = -motor_leg3_j1_dot; + lowState.motorState[9].ddq = lowState.motorState[9].ddq_raw = 0; + lowState.motorState[10].q = lowState.motorState[10].q_raw = -motor_leg3_j2; + lowState.motorState[10].dq = lowState.motorState[10].dq_raw = -motor_leg3_j2_dot; + lowState.motorState[10].ddq = lowState.motorState[10].ddq_raw = 0; + lowState.motorState[11].q = lowState.motorState[11].q_raw = -motor_leg3_j3; + lowState.motorState[11].dq = lowState.motorState[11].dq_raw = -motor_leg3_j3_dot; + lowState.motorState[11].ddq = lowState.motorState[11].ddq_raw = 0; + + lowState.motorState[12].q = lowState.motorState[12].q_raw = motor_leg2_j0; + lowState.motorState[12].dq = lowState.motorState[12].dq_raw = motor_leg2_j0_dot; + lowState.motorState[12].ddq = lowState.motorState[12].ddq_raw = 0; + lowState.motorState[13].q = lowState.motorState[13].q_raw = -motor_leg2_j1; + lowState.motorState[13].dq = lowState.motorState[13].dq_raw = -motor_leg2_j1_dot; + lowState.motorState[13].ddq = lowState.motorState[13].ddq_raw = 0; + lowState.motorState[14].q = lowState.motorState[14].q_raw = -motor_leg2_j2; + lowState.motorState[14].dq = lowState.motorState[14].dq_raw = -motor_leg2_j2_dot; + lowState.motorState[14].ddq = lowState.motorState[14].ddq_raw = 0; + lowState.motorState[15].q = lowState.motorState[15].q_raw = -motor_leg2_j3; + lowState.motorState[15].dq = lowState.motorState[15].dq_raw = -motor_leg2_j3_dot; + lowState.motorState[15].ddq = lowState.motorState[15].ddq_raw = 0; + + ctr++; + if(ctr>=100) + { + //float r2d = 180.0/M_PI; + //std::cout<<"mcu time = "<ly * 100) / 100.0) << "\t" << ((int)(rockerBtn->lx * 100) / 100.0); + std::cout<< "\t" << ((int)(rockerBtn->ry * 100) / 100.0) << "\t" << ((int)(rockerBtn->rx * 100) / 100.0)<