Closed elpimous closed 1 year ago
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.
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
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.
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
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.