Loading new robot but stuck when launch quad_gazebo.launch

[Czworldy]

Hey,

I wish to load a new robot into this framework. but I get some problems.

• First, I followed the guidelines in renaming the links and joints to be of the correct format and ensuring the fixed joints on the toe is preserved when converting to sdf.
• I have also edited the launch file load_robot_params.launch to include option for robot_type 'x20'.
• Creat a yaml named 'x20.yaml'
• And i following the issue https://github.com/robomechanics/quad-sdk/issues/310 #310 #300, to change the stand_joint_angles and the sit_joint_angles and the torque_limit in 'x20.yaml'

The problem is that i can't let the ros controller run. it always stuck here

[ WARN] [1689258661.694966676]: The root link body has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1689258661.704924753]: Grid map visualization node started.
[ INFO] [1689258661.709748848]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'z_points'.
[ INFO] [1689258661.709786004]: grid_map_visualizations: Configured visualization of type 'occupancy_grid' with name 'elevation_grid'.
[ INFO] [1689258661.709807515]: grid_map_visualizations: Configured visualization of type 'vectors' with name 'surface_normals'.
[ INFO] [1689258661.713339402]: Grid map visualization initialized.
[ WARN] [1689258661.715329537]: The root link body has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1689258661.735633312]: Could not find the parameter: `algorithm`. Setting to default value: 'area'.
[ WARN] [1689258661.736750986]: Could not find the parameter: `parallelization_enabled`. Setting to default value: 'false'.
[ WARN] [1689258661.736778713]: Could not find the parameter: `thread_number`. Setting to default value: 'automatic'.
[ WARN] [1689258661.736825297]: Could not find the parameter: `algorithm`. Setting to default value: 'area'.
[ WARN] [1689258661.736837729]: Could not find the parameter: `parallelization_enabled`. Setting to default value: 'false'.
[ WARN] [1689258661.736850091]: Could not find the parameter: `thread_number`. Setting to default value: 'automatic'.
/home/yjy/github/quad_sdk_ws/src/quad-sdk/quad_simulator/gazebo_scripts/worlds/mhz_easy/mhz_easy.ply
[ INFO] [1689258661.781098410]: Can't find param robot_driver/estimator on rosparam server, loading default value.
[ INFO] [1689258661.795871852]: Loading sim robot driver
[INFO] [1689258661.977055, 0.000000]: Waiting for /clock to be available...
[INFO] [1689258661.998045, 0.000000]: Loading model XML from ros parameter robot_description_sdf
[INFO] [1689258662.006735, 0.000000]: Waiting for service /gazebo/spawn_sdf_model
[ INFO] [1689258662.060083737]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1689258662.060900045]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1689258662.181715998]: Number of polygons: 3064
[ INFO] [1689258662.182412457]: Created map with size 10.550000 x 4.000000 m (211 x 80 cells).
[ INFO] [1689258662.182584896]: Published a grid map message.
[ INFO] [1689258662.182637627]: Loaded the mesh from file: /home/yjy/github/quad_sdk_ws/src/quad-sdk/quad_simulator/gazebo_scripts/worlds/mhz_easy/mhz_easy.ply. Its frame_id is set to 'map'
[ INFO] [1689258663.265163877]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1689258663.282920740, 0.007000000]: Physics dynamic reconfigure ready.
[INFO] [1689258663.514788, 0.232000]: Calling service /gazebo/spawn_sdf_model
[INFO] [1689258663.526906, 0.245000]: /clock is published. Proceeding to load the controller(s).
[INFO] [1689258663.527964, 0.246000]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1689258663.788535, 0.359000]: Spawn status: SpawnModel: Successfully spawned entity
[robot_1/spawn_sdf_model-6] process has finished cleanly
log file: /home/yjy/.ros/log/e3af79ee-2189-11ee-a659-c98a0ca8960e/robot_1-spawn_sdf_model-6*.log

And this confused me. when the robot_type=a1 or spirit it looks like everything is okay.

I am not sure if there is something wrong with my sdf file or i miss some steps.

And here this my sdf and x20.yaml file if these will help. x20.sdf.txt x20.yaml.txt

Thanks a lot!

[Czworldy]

update! as long as i change the line in load_robot_params.launch <param name="robot_description_sdf" textfile="$(find x20_description)/sdf_mesh/x20.sdf" /> to <param name="robot_description_sdf" textfile="$(find a1_description)/sdf_mesh/a1.sdf" /> and other thing remain the same, it can start the controller

so i think it seems the problem maybe cause by sdf file.

But, How?

[Czworldy]

and here is my urdf tree x20.pdf

[ologandavid]

Hi @Czworldy, Thanks for using Quad-SDK. Based on what you wrote the controller starts now, but what exactly is the other issue here? Does your robot not appear in the interface? It might be helpful if we could see what code you've changed to better pinpoint your issue?

Best, David Ologan

[Czworldy]

Hi @ologandavid, Thanks for your reply.

this picture shows we i run

roslaunch quad_utils quad_gazebo.launch robot_type:=a1

it seems everything is okay.

The a1 robot appears in the Rviz interface.

But when it comes to my robot x20, i run

roslaunch quad_utils quad_gazebo.launch robot_type:=x20

the robot description is loaded in Rviz but no tf message and the controller doesn't run.

and i don't change any code, my quad-sdk branch is noetic-devel, i just follow the steps i mentioned above.

[Czworldy]

update! sometimes i get

[INFO] [1689321357.122884, 0.214000]: Calling service /gazebo/spawn_sdf_model
[INFO] [1689321357.193053, 0.284000]: /clock is published. Proceeding to load the controller(s).
[INFO] [1689321357.194394, 0.285000]: Controller Spawner: Waiting for service controller_manager/load_controller
[Wrn] [ModelDatabase.cc:340] Getting models from[http://models.gazebosim.org/]. This may take a few seconds.
[INFO] [1689321357.474889, 0.368000]: Spawn status: SpawnModel: Successfully spawned entity
[Msg] Waiting for model database update to complete...
[robot_1/spawn_sdf_model-7] process has finished cleanly
log file: /home/yjy/.ros/log/db6f1e64-221b-11ee-af9c-4d92e6c49eb8/robot_1-spawn_sdf_model-7*.log
[WARN] [1689321387.345072, 0.368000]: Controller Spawner couldn't find the expected controller_manager ROS interface.
[robot_1/controller_spawner-9] process has finished cleanly
log file: /home/yjy/.ros/log/db6f1e64-221b-11ee-af9c-4d92e6c49eb8/robot_1-controller_spawner-9*.log

when i using robot_type=x20 i think

[WARN] [1689321387.345072, 0.368000]: Controller Spawner couldn't find the expected controller_manager ROS interface.

this line means the controller has not be loaded.

[Czworldy]

update! i succes load the x20 model in gazebo. i find the key reason is the <uri>model://x20_description/meshes/x20_shank.stl</uri> in the sdf file. my GAZEBO_MODEL_PATH has <path_to_quad_simulation/x20_description> so i change the uri to <uri>model:///meshes/x20_shank.stl</uri> that makes me load success.

But here is a new problem. I can not let robot stand. i changed the params in x20.yaml like this, since the mass of the robot is about 50kg.

robot_driver:
  sit_kp: [500, 500, 500]
  sit_kd: [10, 10, 10]
  stand_kp: [1000, 1000, 1000]
  stand_kd: [10, 10, 10]
  stance_kp: [400, 400, 400]
  stance_kd: [20, 20, 20]
  swing_kp: [400, 400, 400]
  swing_kd: [20, 20, 20]
  swing_kp_cart: [0, 0, 0] # N/m
  swing_kd_cart: [0, 0, 0] # N/m
  safety_kp: [0, 0, 0]
  safety_kd: [2, 2, 2]
  stand_joint_angles: [0.0, -0.95, 1.78]
  sit_joint_angles: [0.0, -1.13, 2.47]
  torque_limit: [100, 200, 200]

but i when i run

rostopic pub /robot_1/control/mode std_msgs/UInt8 "data: 1"

the front legs can stand but rear legs can not. it looks like

note that the warnings come out from the terminal, it seems like i send the huge torque to rebot joint 3 and 7 but it does not work.

i also check the topic /robot_1/control/joint_command in joint 7

        header: 
          seq: 0
          stamp: 
            secs: 0
            nsecs:         0
          frame_id: ''
        pos_setpoint: 1.78
        vel_setpoint: 0.0
        kp: 1000.0
        kd: 10.0
        torque_ff: 0.0
        pos_component: -482.3105558447456
        vel_component: 12.422408483274353
        fb_component: -469.8881473614712
        effort: -200.0
        fb_ratio: 1.0

and /robot_1/joint_states

name: 
  - '0'
  - '1'
  - '10'
  - '11'
  - '2'
  - '3'
  - '4'
  - '5'
  - '6'
  - '7'
  - '8'
  - '9'
position: [-0.9321272703154309, 1.8089007380909479, 0.004188684139320387, 0.017507260622446452, -0.9484911928664692, 2.2680138602508606, -0.9326113172298331, 1.8084160121918273, -0.9480355828526355, 2.26756122727134, 0.003729556552637625, 0.018004219224918572]
velocity: [0.016783042595236494, -0.04494482272039153, -0.00017011771111890298, -0.0005992993707095018, 0.004493856328822502, -0.06420359400217336, 0.0226586240126376, -0.05838473739936881, 0.004652882205485776, -0.06379406675556024, -6.786838312334401e-05, -0.000928975687585813]
effort: [-18.040560117404468, -28.45128987097015, -4.186982983516779, -17.501267604008465, -1.5537457205360048, -32.0, -17.61526900546077, -27.832164817939486, -2.010945957891861, -32.0, -3.728877847503029, -17.99492948959959]

i don't kown why just those two joints behave so weird.

[Czworldy]

in topic /robot_1/joint_states i find that joint 3 and 7 get effort -32.0 why?

[Czworldy]

Maybe i need to use CasADi's code-generation in nmpc_controller/scrpts to generate my robot's dynamics? Just like #312 said?

[Czworldy]

okay i change controller_plugin.cpp line 39 to torque_lims_ = {100, 100, 100}; it can stand up.

but another problems come out. i use CasADi's code-generation to generate my robot's dynamics C++ code and put it in /gen and change the quad_nlp_utils.cpp to load them.

  // Load basic leg controller functions for the x20 platform
  eval_vec_[X20][FUNC] = eval_g_x20;
  eval_vec_[X20][JAC] = eval_jac_g_x20;
  eval_vec_[X20][HESS] = eval_hess_g_x20;
  eval_work_vec_[X20][FUNC] = eval_g_x20_work;
  eval_work_vec_[X20][JAC] = eval_jac_g_x20_work;
  eval_work_vec_[X20][HESS] = eval_hess_g_x20_work;
  eval_incref_vec_[X20][FUNC] = eval_g_x20_incref;
  eval_incref_vec_[X20][JAC] = eval_jac_g_x20_incref;
  eval_incref_vec_[X20][HESS] = eval_hess_g_x20_incref;
  eval_decref_vec_[X20][FUNC] = eval_g_x20_decref;
  eval_decref_vec_[X20][JAC] = eval_jac_g_x20_decref;
  eval_decref_vec_[X20][HESS] = eval_hess_g_x20_decref;
  eval_checkout_vec_[X20][FUNC] = eval_g_x20_checkout;
  eval_checkout_vec_[X20][JAC] = eval_jac_g_x20_checkout;
  eval_checkout_vec_[X20][HESS] = eval_hess_g_x20_checkout;
  eval_release_vec_[X20][FUNC] = eval_g_x20_release;
  eval_release_vec_[X20][JAC] = eval_jac_g_x20_release;
  eval_release_vec_[X20][HESS] = eval_hess_g_x20_release;
  eval_sparsity_vec_[X20][FUNC] = eval_g_x20_sparsity_out;
  eval_sparsity_vec_[X20][JAC] = eval_jac_g_x20_sparsity_out;
  eval_sparsity_vec_[X20][HESS] = eval_hess_g_x20_sparsity_out;

but as long as i run the local controller can not solve normal. the robot can not stand there stably.

i find that desired_height in local_planner.yaml and some foot position param in the code, and i changed them, but it does not work. My question is that if i load a large mass robot (more than 50kg, i.e. x20 i load) which the size and the mass of this robot is totally different from the a1 and spirit what params should i watch out?

//in  quad_nlp.cpp line 35
  if (default_system_ == X20) {
    for (int i = 0; i < N_; ++i) {
      foot_pos_body_.row(i) << -0.292, -0.18, -0.4, -0.292, 0.18, -0.4,
          0.292, -0.18, -0.4, 0.292, 0.18, -0.4;
      foot_pos_world_.row(i) << 0.292, 0.18, 0, -0.292, 0.18, 0, 0.292,
          -0.18, 0, -0.292, -0.18, 0;
    }
  }

[Czworldy]

Hey, any ideas? @ologandavid I'm looking forward to your reply! Thanks a lot!

Best

[Czworldy]

okay ,i will close this issue and start new one, since the question is changed.

[nanbwrn]

hi,@Czworldy How did you manage to successfully control the X20 robot to stand up eventually? I am currently facing issues with the larger robot failing to stand up despite loading.