Abnormal swing trajectories for back legs when trying to run ocs2_anymal_perceptive on Unitree B1 and Unitree Aliengo

Description

I am trying to integrate the ocs2_perceptive_anymal in the ocs2_robotics_example framework for the Unitree B1 and Unitree Aliengo quadruped robots. Initially, while the front legs' foot placements appeared correct and directly below the hips as expected, the back legs' placements were noticeably wider apart. This was distinctly different from the behavior observed when running the same code with the ANYmal robot, where the foot placements for all legs were directly below the hips in the gravity-aligned direction. I increased the motion tracking weight for the abduct joint, this improved the results but the optimized swing trajectories still look very weird for the back two legs. The swing trajectories seem to be going in the side-ways direction which is causing the robot's back leg to go wider.

Steps to Reproduce

Within the ocs2_anymal_mpc package, replicate the mpc.launch file for use with the Unitree B1 robot.
Modify the robot_description parameter within the launch file to match the Unitree B1 robot's URDF model.
Create a new task.info file tailored for the Unitree B1 robot, including the correct initialRobotState, as well as the appropriate joint limits.
Update the targetCommand.info file to reflect the Unitree B1 robot's specific center of mass height (comHeight) and defaultJointState.
Launch the motion planning and control sequence using the updated configuration files.

Expected Behavior

The foot placements for all legs of the Unitree B1 robot should be directly below the hip joints in the gravity-aligned direction, similar to how it works for the ANYmal robot.

Actual Behavior

The foot placements of the back legs (leg 2 and leg 3) are significantly wider than expected, which is not the case for the front legs (leg 0 and leg 1). Upon increasing the motion tracking weight for the abduct motor, the legs are less wide but the generated swing trajectories looks abnormal as seen from the video.

Supporting Material

[Screenshots of Unitree B1 with incorrect foot placements]
[Screenshot of ANYmal with correct foot placements]
Screenshot of terminal output of nominal foothold terrain printed from selectNominalFootholdTerrain() function inside SwingTrajectoryPlanner.cpp
Video showing abnormal swing trajectories for Unitree B1 while doing trotting gait

https://github.com/leggedrobotics/ocs2/assets/18585930/4bce3249-ecd4-4ad4-8b3a-41690f6d4ab3

Hypothesis/Findings

Upon printing the nominal foothold positions, I noted that:

For legs 0 and 1 (front legs), the y-values are approximately 0.19, which seems correct.
For legs 2 and 3 (back legs), the initial foothold position (I believe it's the current position for legs in contact) appears incorrect, while subsequent positions are correct, as seen from the attached screenshot.
The Future footholds & EE Trajectories optimized by the MPC looks abnormal, which is causing the above behavior

Request for Assistance

@rubengrandia @farbod-farshidian Could you please provide some guidance on how to troubleshoot this issue? I am not sure if it's something to do with the difference in leg configuration of ANYmal and the Unitree robots or, if it's some other parameter tuning issue.

leggedrobotics / ocs2