Enable Gazebo simulation for mycobot280 and with Moveit

[tongtybj]

I found the latest repository does not support gazebo simulation and further moveit. So I correct the urdf model as well as several config files to realize the simulation

Features

• Correct URDF model for mycobot280, in terms of the naming rule for link (e.g., joint1 -> link1)
• Enable gazebo simulation by adding the <transmission> tag in urdf flle and controller plugin for joint trajectory controller
• Enable moveit based joint trajectory operation on RVIZ

https://user-images.githubusercontent.com/3666095/146343975-f724d1dc-4902-43d3-86fd-9a5b1c3b5b31.mp4

Usage:

$ roslaunch mycobot_280_moveit demo_gazebo.launch

[mostlyjason]

@tongtybj Thanks for the PR! I'm very interested in getting this working but I ran into a few errors. I'm still quite new to ROS, so I apologize if any of these are due to my own lack of knowledge/experience. I'm testing this using Noetic in Docker (see https://github.com/elephantrobotics/mycobot_ros/pull/49).

One of the errors I see in rosout is this:

nvidia-ros_1  | [ERROR] [1640892006.646207007, 0.472000000]: Could not load controller 'arm_controller' because controller type 'position_controllers/JointTrajectoryController' does not exist.
nvidia-ros_1  | [ERROR] [1640892006.646231367, 0.473000000]: Use 'rosservice call controller_manager/list_controller_types' to get the available types
nvidia-ros_1  | [ERROR] [1640892007.647415, 1.472000]: Failed to load arm_controller

I resolved this by adding joint_trajectory_controller as a run dependency in the package.xml file. This seems to resolve the error.

<run_depend>joint_trajectory_controller</run_depend>

I'm also seeing some error messages about p gain:

nvidia-ros_1  | You can start planning now!
nvidia-ros_1  | 
nvidia-ros_1  | [spawn_gazebo_model-4] process has finished cleanly
nvidia-ros_1  | log file: /root/.ros/log/46649a06-69a8-11ec-a51f-0242ac130002/spawn_gazebo_model-4*.log
nvidia-ros_1  | [ERROR] [1640893202.648274701, 0.179000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/joint1
nvidia-ros_1  | [ERROR] [1640893202.648650873, 0.179000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/joint2
nvidia-ros_1  | [ERROR] [1640893202.649098924, 0.179000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/joint3
nvidia-ros_1  | [ERROR] [1640893202.649418632, 0.179000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/joint4
nvidia-ros_1  | [ERROR] [1640893202.649760769, 0.179000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/joint5
nvidia-ros_1  | [ERROR] [1640893202.650091986, 0.179000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/joint6
nvidia-ros_1  | [INFO] [1640893202.652785, 0.180000]: Controller Spawner: Waiting for service controller_manager/switch_controller
nvidia-ros_1  | [INFO] [1640893202.654083, 0.181000]: Controller Spawner: Waiting for service controller_manager/unload_controller
nvidia-ros_1  | [INFO] [1640893202.655242, 0.182000]: Loading controller: joint_state_controller
nvidia-ros_1  | [INFO] [1640893202.660253, 0.188000]: Loading controller: arm_controller
nvidia-ros_1  | [INFO] [1640893202.687840, 0.215000]: Controller Spawner: Loaded controllers: joint_state_controller, arm_controller
nvidia-ros_1  | [INFO] [1640893202.689758, 0.217000]: Started controllers: joint_state_controller, arm_controller

I see the p gain set in mycobot_280_moveit/config/gazebo_controllers.yaml and it appears to be loading the arm_controller. I'm not sure why I'm seeing this error or if I can ignore it.

Next, I tried to plan and execute a movement in Rviz. I see Rviz updating the robot position, but I don't see any movement in Gazebo and I see this error:

nvidia-ros_1  | [ERROR] [1640893532.752533513, 329.874000000]: Unable to identify any set of controllers that can actuate the specified joints: [ joint1 joint2 joint3 joint4 joint5 joint6 ]
nvidia-ros_1  | [ERROR] [1640893532.752556901, 329.874000000]: Known controllers and their joints:
nvidia-ros_1  | 
nvidia-ros_1  | [ERROR] [1640893532.752577635, 329.874000000]: Apparently trajectory initialization failed

I'll try to spend more time time testing/debugging this later.

[tongtybj]

@mostlyjason

Thank you for your trial!

I resolved this by adding joint_trajectory_controller as a run dependency in the package.xml file. This seems to resolve the error.

Thank you for your suggestion. I addressed this problem by 2fbeee4

I see the p gain set in mycobot_280_moveit/config/gazebo_controllers.yaml and it appears to be loading the arm_controller. I'm not sure why I'm seeing this error or if I can ignore it.

Actually, this is a message to announce the usage of position-based joint, rather than an error. You can ignore this part. This Q&A explains about this wired behavior in details.

The reason why I choose the position-based joint and related control rather than effort-based one, is I am tired to tune the control parameter for this robot in simulation mode, which is quite meaningless. If you have any need to try effort-based behavior, let's develop in other PR.

Next, I tried to plan and execute a movement in Rviz. I see Rviz updating the robot position, but I don't see any movement in Gazebo and I see this error:

Oh, this is the critical error... The second line of the error message shows there is no any valid joint controllers. I will also try this repository in noetic environment to figure out the reason.

[mostlyjason]

I scrolled up a little further and saw this error in my rosout:

nvidia-ros_1  | [FATAL] [1640947863.064703122]: Exception while loading controller manager 'moveit_simple_controller_manager/MoveItSimpleControllerManager': According to the loaded plugin descriptions the class moveit_simple_controller_manager/MoveItSimpleControllerManager with base class type moveit_controller_manager::MoveItControllerManager does not exist. Declared types are  moveit_fake_controller_manager/MoveItFakeControllerManager

I resolved this error by adding a run dependency to package.xml:

<run_depend>moveit_simple_controller_manager</run_depend>

Now it works fine and I can see the robot position is updated in Rviz and Gazebo! I also tested this in Melodic and its working there too.

I'm unsure why this dependency is needed in Noetic but not Melodic, but it looks like they added it to their template a while back https://github.com/ros-planning/moveit/pull/2455.

I see another error in rosout with Noetic.

roslaunch-7939abf33a6d-1.log:[roslaunch][ERROR] 2021-12-30 20:02:56,871: Cannot locate node of type [state_publisher] in package [robot_state_publisher]. Make sure file exists in package path and permission is set to executable (chmod +x)
roslaunch-7939abf33a6d-1.log:[roslaunch][ERROR] 2021-12-30 20:02:56,871: ERROR: cannot launch node of type [robot_state_publisher/state_publisher]: Cannot locate node of type [state_publisher] in package [robot_state_publisher]. Make sure file exists in package path and permission is set to executable (chmod +x)

According to https://answers.ros.org/question/357672/cannot-locate-node-of-type-state_publisher/ I just needed to rename the publisher in mycobot_280_moveit/launch/gazebo.launch to robot_state_publisher. This removes the error in Noetic and doesn't seem to affect Melodic.

<node name="$(anon robot_state_publisher)" pkg="robot_state_publisher" type="robot_state_publisher" />

However, it results in lots of warnings in Noetic.

nvidia-ros_1  | [ WARN] [1640951848.876142245, 151.734000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame link7 at time 151.703000 according to authority unknown_publisher

This might be related to the deprecation of tf_prefix in Noetic https://answers.ros.org/question/388195/tf_repeated_data-warnings/. I haven't figured out how to add the prefix in to make the warnings go away. I'm not sure its something that should block this PR though since its just a warning, and it doesn't seem to affect the behavior I see in Gazebo.

[tongtybj]

@mostlyjason

Thanks for your great contribution!!

The lack of dependency description of moveit_simple_controller_manager also caused the same error in melodic env (build in docker). This is fixed in a379d71

The noise TF_REPEATED_DATA warning log is because the duplicated launch of robot_state_publish(er) in both gazebo.launch and demo_gazebo.launch. This is fixed in 085543ad302c968d2909f8810d1bcba78ac643fb.

[mostlyjason]

This works great, the warnings are now gone. Thanks for your help on this!

[tongtybj]

Enable gazebo and moveit planning for MyCobot Pro 320 model.

$ roslaunch mycobot_320_moveit demo_gazebo.launch

[mostlyjason]

@tongtybj thanks for doing the 320 model too! I'm also interested in using that one. I tried it and I noticed there appears to be some jittering or instability in the model. When I spawn the robot the texture of the base appears to flash in Gazebo. I also notice that the robot pose drifts over time, even when there is no force is applied. Do you see that as well?

[fedorenchik]

This was closed by GitHub automatically. Should probably be merged.