search

projectatctu / ocs2_fun

ROS package for controlling Anymal and Spot in Gazebo using the OCS2 toolbox
40 stars 2 forks source link

How to get the program running? #3

Closed xu-yang16 closed 7 months ago

xu-yang16 commented 8 months ago

Thanks for your amazing work, but I have no idea how to get it running. I am using the main branch and I failed to run with the following command:

roslaunch ocs2_anymal_robot simple_gazebo.launch

However, the launch file ocs2_anymal_robot/launch/simple_gazebo.launch specifies the initial z pos as 3.0 meter, which causes the robot falling and the program collapsing. I want to know whether the command I used to run was correct. Thanks so much!

The error information is attached as follows:

convex_plane_decomposition_ros_node: /home/caslx/documents/traj_opt/catkin_ocs2_fun/src/ocs2_fun/dependencies/grid_map/grid_map_core/src/GridMap.cpp:87: void grid_map::GridMap::add(const string&, const Matrix&): Assertion `size_(0) == data.rows()' failed.
[convex_plane_decomposition_ros-8] process has died [pid 2311568, exit code -6, cmd /home/caslx/documents/traj_opt/catkin_ocs2_fun/devel/lib/convex_plane_decomposition_ros/convex_plane_decomposition_ros_node __name:=convex_plane_decomposition_ros __log:=/home/caslx/.ros/log/44b9d2c0-d643-11ee-a897-f55b03c22a08/convex_plane_decomposition_ros-8.log].
log file: /home/caslx/.ros/log/44b9d2c0-d643-11ee-a897-f55b03c22a08/convex_plane_decomposition_ros-8*.log
lnotspotl commented 8 months ago

Hi, sorry for getting to you only after two weeks.

Yes, the command that you used, namely roslaunch ocs2_anymal_robot simple_gazebo.launch, looks fine to me and should work. I tested it out on my machine and it works just fine.

Regarding the error, I am not quite sure what causes it. Could you try running the following, similar command?

roslaunch ocs2_anymal_robot simple_gazebo.launch z:=1.5

This command will spawn the anymal robot in a height of 1.5 meters above the ground, not 3 meters.

Edit: I agree that our README.md file is not the most informative and we should absolutely make it more user-friendly. This is still a work-in-progress project, though, so making it easy for other people to use was not our initial goal. Better description coming soon.

xu-yang16 commented 8 months ago

Thanks for your reply.

I am using the main branch. There are some issues I want to identify.

  1. I think the CentralControler includes two types of controller: StaticController and WBC. They can be switched manually via the topic "/changecontroller". However, when the StaticController is initializing, the variable `controllerTypeis not set. Therefore, the functionStaticController::getCommandMessagereturns NULL, which brings an error. To get the controller working, I set thecontrollerType_` to "SIT" when iniatializing.
  2. The URDF of anymal_c uses the same name for links and joints (such as RH_HAA etc.), which caused some errors in my case. Therefore, I renamed those links. This issue is not a major problem.
  3. I set the parameter blind to true and begin running with the following command. (I attached a video here) 
    
# In a terminal
source devel/setup.bash
roslaunch ocs2_anymal_robot simple_gazebo.launch z:=1.5 world:=normal

In the gait command terminal

input trot

In another terminal

source devel/setup.bash rostopic pub /change_controller std_msgs/String "data: 'STAND'" --once # The robot stands up as expected. rostopic pub /change_controller std_msgs/String "data: 'WBC'" --once # The robot begins to oscillate erratically before ultimately collapsing.

lnotspotl commented 8 months ago

Our code is far from perfect, so contributions and small tweaks are definitely welcome.

Unfortunately, I do not have access to the video, you may have forgotten to change the access permissions.

We did have similar issues with the robot starting to oscillate and going crazy, I will talk about this issue after I will have seen the video.

xu-yang16 commented 8 months ago

Sorry. I changed the access permission and you should be able to watch the video now.

lnotspotl commented 8 months ago

Could you possibly create a rosbag and record all the topics such that I can verify that all the necessary information is being communicated to the individual nodes?

Edit: @xu-yang16 the second maintainer, @KortexXD, is more knowledgeable than I am and will help you out with this issue.

xu-yang16 commented 7 months ago

I attached a rosbag file here and the command rosbag info 2024-03-13-14-11-59.bag gives the following output. It seems that even though I set the parameter blind to true, the camera is still working. But I guess that is not a big issue nevertheless.

path:        2024-03-13-14-11-59.bag
version:     2.0
duration:    27.9s
start:       Mar 13 2024 14:12:00.04 (1710310320.04)
end:         Mar 13 2024 14:12:27.95 (1710310347.95)
size:        3.3 GB
messages:    94877
compression: none [873/873 chunks]
types:       diagnostic_msgs/DiagnosticArray       [60810da900de1dd6ddd437c3503511da]
             dynamic_reconfigure/Config            [958f16a05573709014982821e6822580]
             dynamic_reconfigure/ConfigDescription [757ce9d44ba8ddd801bb30bc456f946f]
             gazebo_msgs/LinkStates                [48c080191eb15c41858319b4d8a609c2]
             gazebo_msgs/ModelStates               [48c080191eb15c41858319b4d8a609c2]
             gazebo_msgs/PerformanceMetrics        [884f71fd5037b886ec5e126b83c4425a]
             geometry_msgs/PoseArray               [916c28c5764443f268b296bb671b9d97]
             ocs2_gazebo/JointCommandArray         [b2c52b67c2f09045ab969e8432fb1f2a]
             ocs2_gazebo/RobotState                [9c6fb24a2178a03e3878a5d4a78d8e5b]
             ocs2_msgs/mode_schedule               [9aa75d28b36c292b797e36e87614038a]
             ocs2_msgs/mpc_flattened_controller    [9355aadb2928f436a41799dd737c0f53]
             ocs2_msgs/mpc_observation             [c4075a25799f2a89c6d62b26e93cb66f]
             ocs2_msgs/mpc_target_trajectories     [2d8da69ddfcd018a998edad053f8c7b6]
             rosgraph_msgs/Clock                   [a9c97c1d230cfc112e270351a944ee47]
             rosgraph_msgs/Log                     [acffd30cd6b6de30f120938c17c593fb]
             sensor_msgs/CameraInfo                [c9a58c1b0b154e0e6da7578cb991d214]
             sensor_msgs/CompressedImage           [8f7a12909da2c9d3332d540a0977563f]
             sensor_msgs/Image                     [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/PointCloud2               [1158d486dd51d683ce2f1be655c3c181]
             std_msgs/Bool                         [8b94c1b53db61fb6aed406028ad6332a]
             std_msgs/String                       [992ce8a1687cec8c8bd883ec73ca41d1]
             tf2_msgs/TFMessage                    [94810edda583a504dfda3829e70d7eec]
             theora_image_transport/Packet         [33ac4e14a7cff32e7e0d65f18bb410f3]
             visualization_msgs/Marker             [4048c9de2a16f4ae8e0538085ebf1b97]
             visualization_msgs/MarkerArray        [d155b9ce5188fbaf89745847fd5882d7]
topics:      /anymal_joint_controller/joint_controller/switched_model/command      7390 msgs    : ocs2_gazebo/JointCommandArray        
             /anymal_mpc_mode_schedule                                                1 msg     : ocs2_msgs/mode_schedule              
             /anymal_mpc_observation                                                 29 msgs    : ocs2_msgs/mpc_observation            
             /anymal_mpc_policy                                                      26 msgs    : ocs2_msgs/mpc_flattened_controller   
             /anymal_mpc_target                                                      50 msgs    : ocs2_msgs/mpc_target_trajectories    
             /camera/depth/camera_info                                               72 msgs    : sensor_msgs/CameraInfo               
             /camera/depth/image_raw                                                 72 msgs    : sensor_msgs/Image                    
             /camera/depth/points                                                    72 msgs    : sensor_msgs/PointCloud2              
             /camera/parameter_descriptions                                           1 msg     : dynamic_reconfigure/ConfigDescription
             /camera/parameter_updates                                                1 msg     : dynamic_reconfigure/Config           
             /camera/rgb/camera_info                                                 72 msgs    : sensor_msgs/CameraInfo               
             /camera/rgb/image_raw                                                   70 msgs    : sensor_msgs/Image                    
             /camera/rgb/image_raw/compressed                                        69 msgs    : sensor_msgs/CompressedImage          
             /camera/rgb/image_raw/compressed/parameter_descriptions                  1 msg     : dynamic_reconfigure/ConfigDescription
             /camera/rgb/image_raw/compressed/parameter_updates                       1 msg     : dynamic_reconfigure/Config           
             /camera/rgb/image_raw/compressedDepth/parameter_descriptions             1 msg     : dynamic_reconfigure/ConfigDescription
             /camera/rgb/image_raw/compressedDepth/parameter_updates                  1 msg     : dynamic_reconfigure/Config           
             /camera/rgb/image_raw/theora                                            72 msgs    : theora_image_transport/Packet        
             /camera/rgb/image_raw/theora/parameter_descriptions                      1 msg     : dynamic_reconfigure/ConfigDescription
             /camera/rgb/image_raw/theora/parameter_updates                           1 msg     : dynamic_reconfigure/Config           
             /change_controller                                                       2 msgs    : std_msgs/String                       (2 connections)
             /clock                                                               24399 msgs    : rosgraph_msgs/Clock                  
             /diagnostics                                                            13 msgs    : diagnostic_msgs/DiagnosticArray      
             /gazebo/link_states                                                  24301 msgs    : gazebo_msgs/LinkStates               
             /gazebo/model_states                                                 24301 msgs    : gazebo_msgs/ModelStates              
             /gazebo/parameter_descriptions                                           1 msg     : dynamic_reconfigure/ConfigDescription
             /gazebo/parameter_updates                                                1 msg     : dynamic_reconfigure/Config           
             /gazebo/performance_metrics                                            109 msgs    : gazebo_msgs/PerformanceMetrics       
             /left_camera/depth/camera_info                                          72 msgs    : sensor_msgs/CameraInfo               
             /left_camera/depth/image_raw                                            72 msgs    : sensor_msgs/Image                    
             /left_camera/depth/points                                               72 msgs    : sensor_msgs/PointCloud2              
             /left_camera/parameter_descriptions                                      1 msg     : dynamic_reconfigure/ConfigDescription
             /left_camera/parameter_updates                                           1 msg     : dynamic_reconfigure/Config           
             /left_camera/rgb/camera_info                                            72 msgs    : sensor_msgs/CameraInfo               
             /left_camera/rgb/image_raw                                              70 msgs    : sensor_msgs/Image                    
             /left_camera/rgb/image_raw/compressed                                   70 msgs    : sensor_msgs/CompressedImage          
             /left_camera/rgb/image_raw/compressed/parameter_descriptions             1 msg     : dynamic_reconfigure/ConfigDescription
             /left_camera/rgb/image_raw/compressed/parameter_updates                  1 msg     : dynamic_reconfigure/Config           
             /left_camera/rgb/image_raw/compressedDepth/parameter_descriptions        1 msg     : dynamic_reconfigure/ConfigDescription
             /left_camera/rgb/image_raw/compressedDepth/parameter_updates             1 msg     : dynamic_reconfigure/Config           
             /left_camera/rgb/image_raw/theora                                       73 msgs    : theora_image_transport/Packet        
             /left_camera/rgb/image_raw/theora/parameter_descriptions                 1 msg     : dynamic_reconfigure/ConfigDescription
             /left_camera/rgb/image_raw/theora/parameter_updates                      1 msg     : dynamic_reconfigure/Config           
             /lf_foot_contact                                                        16 msgs    : std_msgs/Bool                        
             /lh_foot_contact                                                        19 msgs    : std_msgs/Bool                        
             /ocs2_anymal/currentCollisionSpheres                                   573 msgs    : visualization_msgs/MarkerArray       
             /ocs2_anymal/currentFeetPoses                                          573 msgs    : geometry_msgs/PoseArray              
             /ocs2_anymal/currentState                                              573 msgs    : visualization_msgs/MarkerArray       
             /ocs2_anymal/desiredAngVelTrajectory                                    24 msgs    : visualization_msgs/MarkerArray       
             /ocs2_anymal/desiredBaseTrajectory                                      24 msgs    : visualization_msgs/Marker            
             /ocs2_anymal/desiredFeetTrajectory/LF                                   24 msgs    : visualization_msgs/Marker            
             /ocs2_anymal/desiredFeetTrajectory/LH                                   24 msgs    : visualization_msgs/Marker            
             /ocs2_anymal/desiredFeetTrajectory/RF                                   24 msgs    : visualization_msgs/Marker            
             /ocs2_anymal/desiredFeetTrajectory/RH                                   24 msgs    : visualization_msgs/Marker            
             /ocs2_anymal/desiredFeetVelTrajectory/LF                                24 msgs    : visualization_msgs/MarkerArray       
             /ocs2_anymal/desiredFeetVelTrajectory/LH                                24 msgs    : visualization_msgs/MarkerArray       
             /ocs2_anymal/desiredFeetVelTrajectory/RF                                24 msgs    : visualization_msgs/MarkerArray       
             /ocs2_anymal/desiredFeetVelTrajectory/RH                                24 msgs    : visualization_msgs/MarkerArray       
             /ocs2_anymal/desiredPoseTrajectory                                      24 msgs    : geometry_msgs/PoseArray              
             /ocs2_anymal/desiredVelTrajectory                                       24 msgs    : visualization_msgs/MarkerArray       
             /ocs2_anymal/localTerrain                                               26 msgs    : visualization_msgs/Marker            
             /ocs2_anymal/optimizedPoseTrajectory                                    24 msgs    : geometry_msgs/PoseArray              
             /ocs2_anymal/optimizedStateTrajectory                                   24 msgs    : visualization_msgs/MarkerArray       
             /ocs2_anymal/swing_planner/nominalFootholds_LF                          27 msgs    : geometry_msgs/PoseArray              
             /ocs2_anymal/swing_planner/nominalFootholds_LH                          27 msgs    : geometry_msgs/PoseArray              
             /ocs2_anymal/swing_planner/nominalFootholds_RF                          27 msgs    : geometry_msgs/PoseArray              
             /ocs2_anymal/swing_planner/nominalFootholds_RH                          27 msgs    : geometry_msgs/PoseArray              
             /ocs2_anymal/swing_planner/trajectory_LF                                27 msgs    : visualization_msgs/MarkerArray       
             /ocs2_anymal/swing_planner/trajectory_LH                                27 msgs    : visualization_msgs/MarkerArray       
             /ocs2_anymal/swing_planner/trajectory_RF                                27 msgs    : visualization_msgs/MarkerArray       
             /ocs2_anymal/swing_planner/trajectory_RH                                27 msgs    : visualization_msgs/MarkerArray       
             /rear_camera/depth/camera_info                                          72 msgs    : sensor_msgs/CameraInfo               
             /rear_camera/depth/image_raw                                            72 msgs    : sensor_msgs/Image                    
             /rear_camera/depth/points                                               72 msgs    : sensor_msgs/PointCloud2              
             /rear_camera/parameter_descriptions                                      1 msg     : dynamic_reconfigure/ConfigDescription
             /rear_camera/parameter_updates                                           1 msg     : dynamic_reconfigure/Config           
             /rear_camera/rgb/camera_info                                            72 msgs    : sensor_msgs/CameraInfo               
             /rear_camera/rgb/image_raw                                              70 msgs    : sensor_msgs/Image                    
             /rear_camera/rgb/image_raw/compressed                                   69 msgs    : sensor_msgs/CompressedImage          
             /rear_camera/rgb/image_raw/compressed/parameter_descriptions             1 msg     : dynamic_reconfigure/ConfigDescription
             /rear_camera/rgb/image_raw/compressed/parameter_updates                  1 msg     : dynamic_reconfigure/Config           
             /rear_camera/rgb/image_raw/compressedDepth/parameter_descriptions        1 msg     : dynamic_reconfigure/ConfigDescription
             /rear_camera/rgb/image_raw/compressedDepth/parameter_updates             1 msg     : dynamic_reconfigure/Config           
             /rear_camera/rgb/image_raw/theora                                       73 msgs    : theora_image_transport/Packet        
             /rear_camera/rgb/image_raw/theora/parameter_descriptions                 1 msg     : dynamic_reconfigure/ConfigDescription
             /rear_camera/rgb/image_raw/theora/parameter_updates                      1 msg     : dynamic_reconfigure/Config           
             /rf_foot_contact                                                        17 msgs    : std_msgs/Bool                        
             /rh_foot_contact                                                         3 msgs    : std_msgs/Bool                        
             /right_camera/depth/camera_info                                         72 msgs    : sensor_msgs/CameraInfo               
             /right_camera/depth/image_raw                                           72 msgs    : sensor_msgs/Image                    
             /right_camera/depth/points                                              72 msgs    : sensor_msgs/PointCloud2              
             /right_camera/parameter_descriptions                                     1 msg     : dynamic_reconfigure/ConfigDescription
             /right_camera/parameter_updates                                          1 msg     : dynamic_reconfigure/Config           
             /right_camera/rgb/camera_info                                           72 msgs    : sensor_msgs/CameraInfo               
             /right_camera/rgb/image_raw                                             70 msgs    : sensor_msgs/Image                    
             /right_camera/rgb/image_raw/compressed                                  70 msgs    : sensor_msgs/CompressedImage          
             /right_camera/rgb/image_raw/compressed/parameter_descriptions            1 msg     : dynamic_reconfigure/ConfigDescription
             /right_camera/rgb/image_raw/compressed/parameter_updates                 1 msg     : dynamic_reconfigure/Config           
             /right_camera/rgb/image_raw/compressedDepth/parameter_descriptions       1 msg     : dynamic_reconfigure/ConfigDescription
             /right_camera/rgb/image_raw/compressedDepth/parameter_updates            1 msg     : dynamic_reconfigure/Config           
             /right_camera/rgb/image_raw/theora                                      73 msgs    : theora_image_transport/Packet        
             /right_camera/rgb/image_raw/theora/parameter_descriptions                1 msg     : dynamic_reconfigure/ConfigDescription
             /right_camera/rgb/image_raw/theora/parameter_updates                     1 msg     : dynamic_reconfigure/Config           
             /robot_description/anymal/state                                       7815 msgs    : ocs2_gazebo/RobotState               
             /rosout                                                                473 msgs    : rosgraph_msgs/Log                     (7 connections)
             /rosout_agg                                                            434 msgs    : rosgraph_msgs/Log                    
             /tf                                                                   1146 msgs    : tf2_msgs/TFMessage                   
             /tf_static                                                               1 msg     : tf2_msgs/TFMessage
KortexXD commented 7 months ago

Hi, thanks for the feedback. I've checked the issue you reported, but I'm not sure what caused strange behavior of the robot. I attempted to replicate your problem by creating new environment and using the readme. For me it was running normally here.

Is it possible that you've made some changes in the controller section that can cause this behavior? We encountered an issue when we modified the robot's model, particularly its inertial parameters, which caused oscillation too. Additionally, the initial z position is higher because when you spawn the robot, it needs some time to move joints from the spawn position to the sitting position, I've checked 1.5 m and it works just fine. I currently use Ubuntu 20.04.6 LTS and ros-noetic, so there might be compatibility issues to consider.

You mentioned setting the parameter blind to true. Just to clarify, this parameter isn't related to the cameras themselves, but rather to mapping the surrounding terrain. When this parameter is set to true, the robot doesn't generate an elevation map from any of the cameras, although the cameras remain active. In my video, you can see a purple square under the robot, this represents the elevation map.

lnotspotl commented 7 months ago

Closing for inactivity.