issues on noetic_devel_ekf_clean

[elpimous]

Hello the team :

1/ when using gazebo + rviz on roslaunch quad_utils quad_gazebo.launch, spirit_description, a1_description and ylo2_description added to ~/.bashrc Spirit and A1 play correctly on rviz+gazebo Ylo2 plays ok on rviz, but not under gazebo. Do I miss anything ?

spirit:

a1:

ylo2:

2/ under rviz, foot contacts are not same position on Ylo2 : Where should i investigate ? steps modified for ylo2 robot : (following tuto)

• 4 Implement Custom NMPC Controller Files,
• 5 Update Local Planner,
• 6 Robot Driver.

spirit:

a1:

ylo2:

[elpimous]

3/ should I need to change this too ? https://github.com/robomechanics/quad-sdk/blob/9973ff94a6b28e74fd69b6b51f561b2515f9c59d/nmpc_controller/include/nmpc_controller/quad_nlp.h#L280-L289 not written on tuto : Adding-a-New-Type-of-Robot-to-Quad-SDK

4/ where is it ? :

4. Implement Custom NMPC Controller Files
In /nmpc_controller/src/quad_nlp.cpp
    load basic leg controller functions for the Robot_NAME platform

[elpimous]

Hello the team, @jcnorby @ologandavid ,

any feedbacks with this branch :

testing robot_driver.launch with my robot :

startup_routine Done.
[ WARN] [1697048024.926420505]: No body pose (mocap) recieved
[ INFO] [1697048024.931800137]: t_diff_mb_send = 0.0053
[ WARN] [1697048025.937330400]: No body pose (mocap) recieved

This branch isn't setup with ekf_filter

So, I changed : comp_filter to ekf_filter

https://github.com/robomechanics/quad-sdk/blob/9973ff94a6b28e74fd69b6b51f561b2515f9c59d/robot_driver/src/robot_driver.cpp#L43-L44

and, when launching robot_driver.launch, terminal becomes white (like a permanent cout "")

Any idea ?

[ologandavid]

Hi @elpimous, Any updates? Quad-SDK's default estimator is still the comp filter. You can specify EKF by either directly changing it in the launch file, or feeding it in as an argument during launch.

Best, David Ologan

[elpimous]

Hello @ologandavid Happy to here from you Could you tell me what is the correct branch to use, for a Noetic real robot test, with ekf filtrer ?!

Noetic devel ekf clean didn't work a mont ago, on real (worked on sim) Take care of you my friend Vincent

[ologandavid]

Hi @elpimous, Did it at least work for your robot in sim? There are a couple of things you would have to change to get it working on a real robot. First, you would need to input your robot's start configuration, (standing) to initialize the filter, and some tuning is probably required to see the desired results. You should also stop publishing the imu data from rviz, and instead use your robot's imu. I can push some more changes to that branch to make this process a little easier.

Best, David Ologan

[elpimous]

Yeah. It worked nicely on sim with ylo2.

Oh, I anderstand now why it didn't work as expected on real.

Perhaps could you push a readme_for_real_usage.md ?!

Sure, modifying files could be cool for me, but some of my friend, like me, need to anderstand and test.. (and our brain isn't as accurate as your lol). I'm really happy to see you taking care of your users..

So, the story continues.... Lol

[ologandavid]

Hi @elpimous, Yeah, I can make some changes to the setup to help streamline it and add a readme in estimators.

Best, David Ologan

[elpimous]

@ologandavid Please, perhaps could you informed us, here, when you'll make changes on noeticdevel ekf_clean. we'll test and report our progress here, if you want. We will be patient, for sure.

[elpimous]

@ologandavid Hi, noetic_devel_ekf_clean, on SIMULATION working on ylo2

https://github.com/robomechanics/quad-sdk/assets/8529940/f84bf4b0-9afe-44b7-8bc5-2208bcb72c0f

Well, trajectories are not perfect, i'll work on it later.

Now, let's modify code for HARDWARE run (on real YLO2 robot)

[elpimous]

Questions : 1/ you need to input your robot's start configuration, (standing) to initialize the filter

• Actually I changed sit_joint_angles stand_joint_angles sit_kp sit_kd from quad_util/config/ylo2.yaml Is it what you were saying ?!

2/You should also stop publishing the imu data from rviz, and instead use your robot's imu

• When using is_hardware=true in quad_spawn.launch, the controller runs robot_driver/ylo2_interface
• the variable last_imu_msg_ is feeded with my real IMU. (tested with a simple cout, into Ylo2Interface::recv function It should be enough, correct ?

3/ Anything else to modify ?

THANKS

[ologandavid]

Hi @elpimous, Sorry I haven't quite gotten to this yet. 1) When I say input the robot's start configuration, I mean directly in robot_driver's update_state function. In sim, I grab the robot's start state for the estimation from gazebo (estimated_state = last_robot_state_msg). For hardware, you need to do the same, but instead you need to initialize the robot on the starting stand state of your robot.

2) Regarding the IMU, I modified Spirit's urdf to publish to the imu topic in sim, but if you're using your already grabbing it from your robot you don't need to worry about this part.

Best, David

[elpimous]

@ologandavid 1/ Does it seems correct for you ?

bool RobotDriver::updateState(){
  // If Operating on Hardware
  if (is_hardware_) {
    // grab data from hardware
    bool fully_populated = hardware_interface_->recv(
        last_joint_state_msg_, last_imu_msg_, user_rx_data_);

    ros::Time state_timestamp = ros::Time::now();

    // load robot sensor message to state estimator class
    if (fully_populated) {

      //TODO : added for ylo2 real
      estimated_state_ = last_robot_state_msg_;
      last_robot_state_msg_.body.twist.angular = last_imu_msg_.angular_velocity;
      last_robot_state_msg_.joints = last_joint_state_msg_;
      last_joint_state_msg_.header.stamp = state_timestamp;
      last_imu_msg_.header.stamp = state_timestamp;

      state_estimator_->loadSensorMsg(last_imu_msg_, last_joint_state_msg_);
    }
    else {
      ROS_WARN_THROTTLE(1, "No imu or joint state (robot) recieved");
    }...

2/ could you confirm the necessary launch files to start, to make real robot walk ? using :

• roslaunch quad_utils robot_driver.launch
• roslaunch quad_utils remote_control.launch ( + launch real imu node)
• roslaunch quad_utils quad_plan.launch reference:=twist logging:=true
• rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/robot_1/cmd_vel

robot stays white in rviz, and no GRF arrows on legs, and warnings into quad_plan.launch terminal.

[elpimous]

Hi @ologandavid ... Any help , for 1 and 2 ?

[elpimous]

Hey @ologandavid Small up ?

[elpimous]

Any news, friends ?

[elpimous]

@ologandavid Hello, did you have Time to modify /devel code, for Real usage ? What are the needed nodes, for Real ? Merry Christmas all. Vincent

[ologandavid]

Hi Vincent, Thanks for all your patience. I'm making some changes to the code to make this process a little easier for you. I don't think you should need any additional nodes outside of robot_driver to get estimation working. If you'd like we could schedule a formal meeting to work out some of the issues you've been having.

Happy Holidays, David Ologan

[elpimous]

@ologandavid Hi, cool. Could you tell me when it will be done. About meeting, my english on visio? Would be erratic 😁. I emailed you. Take your Time , and i Hope you'll not forget me. 😅🥳