Closed elpimous closed 5 months ago
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
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
and, when launching robot_driver.launch, terminal becomes white (like a permanent cout "")
Any idea ?
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
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
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
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
Hi @elpimous, Yeah, I can make some changes to the setup to help streamline it and add a readme in estimators.
Best, David Ologan
@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.
@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)
Questions :
1/ you need to input your robot's start configuration, (standing) to initialize the filter
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
is_hardware=true
in quad_spawn.launch, the controller runs robot_driver/ylo2_interfacelast_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
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
@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 :
robot stays white in rviz, and no GRF arrows on legs, and warnings into quad_plan.launch terminal.
Hi @ologandavid ... Any help , for 1 and 2 ?
Hey @ologandavid Small up ?
Any news, friends ?
@ologandavid Hello, did you have Time to modify /devel code, for Real usage ? What are the needed nodes, for Real ? Merry Christmas all. Vincent
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
@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. 😅🥳
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:![spirit](https://github.com/robomechanics/quad-sdk/assets/8529940/02e7aa52-82d9-4037-889b-df2d4f8414f2)
a1:![a1](https://github.com/robomechanics/quad-sdk/assets/8529940/47868449-4f60-49f3-8883-22aabc1f61cf)
ylo2:![ylo2](https://github.com/robomechanics/quad-sdk/assets/8529940/be0bd66c-e5d6-4a93-a367-1b4fd9648a80)
2/ under rviz, foot contacts are not same position on Ylo2 : Where should i investigate ? steps modified for ylo2 robot : (following tuto)
spirit:![Capture d’écran de 2023-09-17 20-18-05](https://github.com/robomechanics/quad-sdk/assets/8529940/da03e67b-cd99-46c6-9948-5f1a22d4abf6)
a1:![Capture d’écran de 2023-09-17 20-19-20](https://github.com/robomechanics/quad-sdk/assets/8529940/0589bb1f-14fc-4671-8c08-5c437a1c9895)
ylo2:![Capture d’écran de 2023-09-17 20-22-22](https://github.com/robomechanics/quad-sdk/assets/8529940/5034aaf8-ddcd-47c2-8d7a-ca5d6c24d0ef)