Add imu subscriber for hardware interface READ() function

[elpimous]

Hello qiayuanliao

I'm impressed with your controller. Think it's exactly what I need for my real robot YLO2. Tested lots of controllers (champ, Wolf, quad-sdk...)

However don't have succes when trying to integrate imu subscriber on the code, to be able to call imu message into read() function of the robot hardware interface.

A1 has imu integrated in it lib, and some of my friends and I need to call external topic imu.

Could you add the imu subscriber integration in legged_unitree_hw code, for example? Perhaps it could extend users and give big help for us 😉

Thanks qiayuanliao. Vincent.

[qiayuanl]

Hi, I am happy that you enjoy this controller. Can you show me your code? I believe that it's easy to add an IMU subscriber on the HardwareInterface, and you may have some bug. I may try to help you find out.

[elpimous]

Hello. What a quick answer...

Humm. I can't call imu message under read() function. here is what i'd like to do :

https://github.com/elpimous/Legged_Control

void Ylo2HW::read(const ros::Time& /*time*/, const ros::Duration& /*period*/) {

  // read the 12 joints, and store values into legged controller
  for (int i = 0; i < 12; ++i) {

    // Reset values
    RX_pos = 0.0;
    RX_vel = 0.0;
    RX_tor = 0.0;
    RX_volt = 0.0;
    RX_temp = 0.0;
    RX_fault = 0.0;

    auto ids  = command.motor_adapters_[i].getIdx();
    int port  = command.motor_adapters_[i].getPort();
    auto sign = command.motor_adapters_[i].getSign();

    // call ylo2 moteus lib
    command.read_moteus_RX_queue(ids, port, 
                                  RX_pos, RX_vel, RX_tor, 
                                  RX_volt, RX_temp, RX_fault);  // query values;

    usleep(10); // TODO : test and reduce pause !

    // ex : jointData_ = [(pos_, vel_, tau_, posDes_, velDes_, kp_, kd_, ff_), (pos_, vel_, tau_, posDes_, velDes_, kp_, kd_, ff_),...]
    jointData_[i].pos_ = static_cast<double>(sign*(RX_pos*2*M_PI)); // joint turns to radians
    jointData_[i].vel_ = static_cast<double>(RX_vel);   // measured in revolutions / s
    jointData_[i].tau_ = static_cast<double>(RX_tor);   // measured in N*m

    // TODO read volt, temp, faults for Diagnostics

    usleep(200); // TODO : test and reduce pause !

  // TODO
  // imu_message contains the real imu topic msg
  // read imu message, and store values into legged controller
  imu_message = imu_callback(); _// read imu full message_

  imuData_.ori_[0]        = imu_message->orientation.x;
  imuData_.ori_[1]        = imu_message->orientation.y;
  imuData_.ori_[2]        = imu_message->orientation.z;
  imuData_.ori_[3]        = imu_message->orientation.w; // TODO : is this 4rd index w, or x ?

  imuData_.angularVel_[0] = imu_message->angular_velocity.x;
  imuData_.angularVel_[1] = imu_message->angular_velocity.y;
  imuData_.angularVel_[2] = imu_message->angular_velocity.z;
  imuData_.linearAcc_[0]  = imu_message->linear_acceleration.x;
  imuData_.linearAcc_[1]  = imu_message->linear_acceleration.y;
  imuData_.linearAcc_[2]  = imu_message->linear_acceleration.z;

  for (size_t i = 0; i < CONTACT_SENSOR_NAMES.size(); ++i) { // "RF_FOOT", "LF_FOOT", "RH_FOOT", "LH_FOOT"
    contactState_[i] = 0.0; // lowState_.footForce[i] > contactThreshold_;
  }

  // Set feedforward and velocity cmd to zero to avoid for safety when not controller setCommand
  std::vector<std::string> names = hybridJointInterface_.getNames();
  for (const auto& name : names) {
    HybridJointHandle handle = hybridJointInterface_.getHandle(name);
    handle.setFeedforward(0.);
    handle.setVelocityDesired(0.);
    handle.setKd(3.);
  }
}

Don't know where to put : void ImuCallback(const sensor_msgs::Imu::ConstPtr& imu_message) and ros::Subscriber sub = n.subscribe("imu/data", 100, ImuCallback); Perhaps, if you have a few time, could you help with commented above lines in your actual code?

USING EXACTLY SAME STRUCTURE AS LEGGED_A1_HW

I'm part of a big quadrupeds community, autodidacts for lots of us, and we nearly all have external imu, built our robot(s), and control robot with small boards or better. (UP Xtreme I7 for me). Our only way is to use a subscriber, correct ?

---

Morever, Do/can I put here my real covariances values ?

under ylo2hw.cpp :

bool Ylo2HW::setupImu() {

  imuData_.oriCov_[0] = 0.0012; // TODO must replace with my real cov ?
  imuData_.oriCov_[4] = 0.0012; // TODO is it usefull to feed all 9 cov ?
  imuData_.oriCov_[8] = 0.0012;

  imuData_.angularVelCov_[0] = 0.0004; // same here...
  imuData_.angularVelCov_[4] = 0.0004;
  imuData_.angularVelCov_[8] = 0.0004;

  return true;
}

Thanks a lot for your help. Vincent

[qiayuanl]

The imuCallback and subscriber should be the member function and variable of Ylo2HW respectively. The subscriber should be initialize in Ylo2HW::init().

Covariances value doesn't in my code yet actually.

[elpimous]

thanks for answer. Well, compilation is ok, but i can't read imu message ?!

As you said, I inserted imu callbacy into init() : https://github.com/elpimous/Legged_Control/blob/fa877fa5458aee1cd26bf31753235fe4137dea6d/legged_unitree_hw/src/UnitreeHW.cpp#L44

But no loop so ?!

here there is the callback : https://github.com/elpimous/Legged_Control/blob/fa877fa5458aee1cd26bf31753235fe4137dea6d/legged_unitree_hw/src/UnitreeHW.cpp#L27-L33

Could you help in imu read example value here ? https://github.com/elpimous/Legged_Control/blob/fa877fa5458aee1cd26bf31753235fe4137dea6d/legged_unitree_hw/src/UnitreeHW.cpp#L106

@qiayuanliao Thanks a lot. Vincent