Customizing the new robot

[Timofeibb]

Hi. I am trying to customize my own robot based on this repository. The problem is that I don't quite understand which files to look at first (task.info, sqp.info, controllers. info etc.). I have already built the robot URDF model, and the final structure is very similar to the ANYmal robot from the repository (joints, links, axes names), the different is on motor effort, speed and joint limits, mass etc. But the main difference is that my robot has a different leg position, it's not >< but >>. I also changed the collision settings in gazebo, I compiled the SDF file from URDF and looked at the contacts
SDF

<collision name='RF_SHANK_fixed_joint_lump__RF_FOOT_collision_1'>
        <pose>0 0 -0.21 0 -0 1.5708</pose>
        <geometry>
          <sphere>
            <radius>0.03</radius>
          </sphere>
        </geometry>
      </collision>

URDF

    <gazebo reference="RF_FOOT">
        <sensor name="rf_foot_contact" type="contact">
            <plugin name="rf_contact_plugin" filename="libContactSensor.so" />
            <contact>
                <collision>RF_SHANK_fixed_joint_lump__RF_FOOT_collision_1</collision>
            </contact>
        </sensor>
    </gazebo>

I have already made changes to file task.info, controllers.info, targetCommand.info etc. to match my robot parameters, but the result is still far from ideal. As soon as I switch the controller from STAND to WBC the robot starts to jerk continuously and just falls over. You can see my simulation in gazebo below

Untitled.video.mp4

These are my MPC settings from the terminal before launch.

#### Rollout Settings: 
 #### =============================================================================
 #### 'AbsTolODE'..........................................................1e-05
 #### 'RelTolODE'..........................................................0.001
 #### 'maxNumStepsPerSecond'...............................................10000
 #### 'timeStep'...........................................................0.015
 #### 'integratorType'.....................................................ODE45
 #### 'checkNumericalStability'................................................0
 #### 'reconstructInputTrajectory'.............................................1 (default)
 #### 'rootFindingAlgorithm'...................................................0 (default)
 #### 'maxSingleEventIterations'..............................................10 (default)
 #### 'useTrajectorySpreadingController'.......................................0 (default)
 #### =============================================================================

 #### Robot Model Settings:
 #### ==================================================
 #### 'algorithm'............................................................SQP
 #### 'phaseTransitionStanceTime'............................................0.4 (default)
 #### 'recompileLibraries'.....................................................1
 #### 'robotName'..........................................................alpha
 #### 'autodiffLibraryFolder'........................................../tmp/ocs2 (default)
 #### 'analyticalInverseKinematics'............................................1
 #### 'frictionCoefficient'..................................................0.5
 #### 'coneRegularization'....................................................25 (default)
 #### 'gripperForce'...........................................................0
 #### 'muFrictionCone'.......................................................0.1
 #### 'deltaFrictionCone'......................................................5
 #### 'muFootPlacement'......................................................0.1
 #### 'deltaFootPlacement'..................................................0.01
 #### 'muSdf'................................................................0.1
 #### 'deltaSdf'...........................................................0.005
 #### 'muJointsPosition'.....................................................0.1
 #### 'deltaJointsPosition'.................................................0.01
 #### 'muJointsVelocity'.....................................................0.1
 #### 'deltaJointsVelocity'..................................................0.1
 #### 'muJointsTorque'.........................................................1
 #### 'deltaJointsTorque'......................................................1
 joint lower limits: -0.62 -2.09 -2.69 -0.62 -2.09 -2.69 -0.62 -2.09 -2.69 -0.62 -2.09 -2.69
 joint upper limits:  0.62  2.79 -0.01  0.62  2.79 -0.01  0.62  2.79 -0.01  0.62  2.79 -0.01
 joint velocity limits: 36 36 22 36 36 22 36 36 22 36 36 22
 joint torque limits: 36.8 36.8 44.4 36.8 36.8 44.4 36.8 36.8 44.4 36.8 36.8 44.4
 #### ================================================ ####

 #### Tacking Cost Weights:
 #### ==================================================
 #### 'roll'.................................................................100
 #### 'pitch'................................................................300
 #### 'yaw'..................................................................300
 #### 'base_position_x'.....................................................1000
 #### 'base_position_y'.....................................................1000
 #### 'base_position_z'.....................................................1500
 #### 'base_angular_vel_x'....................................................10
 #### 'base_angular_vel_y'....................................................30
 #### 'base_angular_vel_z'....................................................30
 #### 'base_linear_vel_x'.....................................................15
 #### 'base_linear_vel_y'.....................................................15
 #### 'base_linear_vel_z'.....................................................30
 #### 'joint_position_HAA'.....................................................2
 #### 'joint_position_HFE'.....................................................2
 #### 'joint_position_KFE'.....................................................1
 #### 'foot_position_x'[ INFO] [1725938789.025028583]: Elevation mapping node started.
.......................................................20
 #### 'foot_position_y'.......................................................20
 #### 'foot_position_z'.......................................................20
 #### 'joint_velocity_HAA'..................................................0.02
 #### 'joint_velocity_HFE'..................................................0.02
 #### 'joint_velocity_KFE'..................................................0.01
 #### 'foot_velocity_x'.......................................................30
 #### 'foot_velocity_y'.......................................................30
 #### 'foot_velocity_z'.......................................................30
 #### 'contact_force_x'....................................................0.001
 #### 'contact_force_y'....................................................0.001
 #### 'contact_force_z'....................................................0.001
 #### ================================================ ####

 #### Swing trajectory Settings:
 #### ==================================================
 #### 'liftOffVelocity'.....................................................0.05
 #### 'touchDownVelocity'..................................................-0.05
 #### 'swingHeight'.........................................................0.06
 #### 'errorGain'..............................................................0
 #### 'swingTimeScale'.......................................................0.1
 #### 'sdfMidswingMargin'..................................................0.005
 #### 'terrainMargin'..........................................................0
 #### 'previousFootholdFactor'.............................................0.333
 #### 'previousFootholdDeadzone'............................................0.05
 #### 'previousFootholdTimeDeadzone'........................................0.25
 #### 'invertedPendulumHeight'...............................................0.5 (default)
 #### 'nominalLegExtension'.................................................0.55 (default)
 #### 'legOverExtensionPenalty'................................................5 (default)
 #### 'referenceExtensionAfterHorizon'.........................................1 (default)
 #### 'maximumReferenceSampleTime'..........................................0.05 (default)
 #### 'swingTrajectoryFromReference'...........................................0 (default)
 #### ==================================================

 #### terrain plane:
 #### ==================================================
 #### 'x'......................................................................0
 #### 'y'......................................................................0
 #### 'z'......................................................................0
 #### 'roll'...................................................................0
 #### 'pitch'..................................................................0
 #### 'yaw'....................................................................0
 #### ==================================================

Before that I already used another repository [https://github.com/qiayuanl/legged_control] and there I successfully customized my own robot with grid map. But there is no grid map navigation function there, it is a kind of blind mpc like in tbai_mpc_blind.

Untitled.video1.mp4

So my question is, what should I pay attention to when developing my own robot using your repository?
Thank you.

[lnotspotl]

Hey @Timofeibb,

it's great to see your efforts to make tbai work with your own robot. We had some similar issues with the MPC controller when we had tried using the urdf of the Spot robot from Boston Dynamics, provided here. If you're interested, we have a video showing how Spot walks using the MPC controller, very similar in nature to what is contained in tbai.

I might be wrong but I think the issue we had was that the urdf's velocity limit for all the joints was too high. Could you try lowering the joint velocity limits to a lower value, say 5 rad/s for each joint?

[Timofeibb]

Hi @lnotspotl,
thanks for your reply, I will check out this repository and reply here later about the results.
btw, I tried reducing the velocity of the joints to 5 rad\s, but the result is still the same.

[lnotspotl]

The issue might also be that your foot's frame is within the collision body while Anymal has that foot reference frame on the tip of the collision body

Not sure, just speculating. I think this might be causing some instabilities in the WBC controller itself.

[Timofeibb]

@lnotspotl
I apologize for the late reply, I was busy with other things.
In general, I did some research and came to the following conclusion: First - the robot model and especially the point of the foot which has contact with the ground is very important for calculating the trajectory of motion (Fig.1). Second - tbai repository is able to calculate the trajectory of motion only for robots with X-shaped legs (aka ANYmal). I made a video with the wrong leg structure for my robot. As expected, everything worked perfectly (Vid.1) and one with the right structure. (Vid. 2)

Fig.1

https://github.com/user-attachments/assets/8b0929a0-e005-4a65-a501-4b3ceac7cb61
Vid.1

https://github.com/user-attachments/assets/c0aa891e-046b-430b-8195-90d92ee6b9d7
Vid.2

The repository ocs2_fun is suitable for designing robots with straight legs <<, but as I have noticed the ocs2_fun is not as good as tbai. In addition, dependencies in ocs2_fun also has custom code in the ocs2_perceptive_anymal folder. There are some additional parts for spot kinematic. This is my implementation of ocs2_fun code.

https://github.com/user-attachments/assets/3481023f-8774-46e8-ba04-0027fbd17e38
Vid.3

I changed the robot spot values in the kinematics section for my robot. It looks good, but not perfect. And since we are talking about ocs2_fun, I would like to ask what the values theta3_offset and theta4_offset in AnalyticalInverseKinematics.cpp mean?

So the question, is there an update planned for using the robot with normal leg placement << in this repository?

And how can tbai be adapted to use these kinds of robots?