FootState calculation

[gearguan]

Hi there, I'm testing quad-sdk on hardware, but there's a problem that's been troubling me. The FootState calculated by robot_driver occasionally goes wrong and the z position of the feet becomes negative, has anyone encountered a similar problem? I read the paper and know how the foot position is calculated, does this phenomenon imply that I have to switch to a more stable method of estimating the foot position?

[jcnorby]

Is this data with online state estimation or external (i.e. mocap)? The foot position calculation should be pretty reliable (assuming your kinematics match the actual robot, so I'll assume that is a Spirit for now), but since the foot position data is in the world frame it is affected by the state estimation and any drift present.

[gearguan]

Is this data with online state estimation or external (i.e. mocap)? The foot position calculation should be pretty reliable (assuming your kinematics match the actual robot, so I'll assume that is a Spirit for now), but since the foot position data is in the world frame it is affected by the state estimation and any drift present.

Hi Joe, Thank you very much for your reply! I didn't use online state estimation. I used T265 tracking camera to publish the mocap topic, to get the position and orientation of the robot body, and for IMU I used LPMS-IG1. I did not test with Spirit robot or Unitree's A1 robot, but I made sure I used the correct URDF model and also generated the dynamics model with a matlab script. I noticed that Robomechanics Lab posted a test video on YouTube where the robot is moving outdoors, is it using the T265 camera as well?

[jrenaf]

Hi @gearguan, the video showing Spirit walking outdoor is indeed using T265 for the estimation. But the visual+IMU solution on legged robots are unstable and inaccurate itself (which might result in lower body height and therefore, with leg kinematic accumulated, toes are a little bit below the gound). Another issue related is the frequency of the estimation topic is published to robot-driver, which would impact the complementary filter coefficients computation within PID control. I think the second issue is more dominant in the unstability of the robot.

[gearguan]

Hi @jrenaf Many thanks for the advice! :) I tested with a standalone PC running the quad-sdk controller as ROS slave and the robot running ROS Master. They are connected with a CAT-6 cable. The signals from the T265, IMU and encoder are sent to the PC as separate topics. We were aware of the possibility of delays, but didn't think it would be a serious problem. Later we will try to transmit these signals directly by means of the UDP protocol.

[jcnorby]

Closing this issue due to lack of activity, also seems like a implementation-specific issue (correct me if wrong). Also t265 data is known to drift quite a bit and particularly during locomotion, and that seems to be what is happening here (10cm of drift over 5 seconds makes sense).

That said, better state estimation/robot frame controls are certainly in the works, so this may be included in a future update.