Position and velocity control on Gazebo simulation

[matteodv99tn]

As of now, the integration with Gazebo Ignition allows the simulation only of the torque (effort) interface.

Upon loading of simulation, the log reports

[INFO] [1745392837.005115761] [gz_ros2_control]: The position_proportional_gain has been set to: 0.1

however I have not been able to find a way to dynamically set it at runtime from URDF or config. file as specified here here.
Apparently, the loading of the parameter from the URDF is a recent change which has not been properly ported back to humble (the version I am using for testing).
Indeed, comparing the rolling plugin implementation with the humble branch, there are minor differences.

A solution could be the use of custom plugin for the interface with Ignition (as in the franka ros2 interface), but this requires some effort and testing.

With this premise, it is currently not possible to simulate, in ignition, using the position command interface. Possible strategies are:

• implementation of pid controller to convert position references to effort to be sent to Gazebo;
• find a way to properly set the position_proportional_gain parameter of the ROS2 control plugin

[matteodv99tn]

Apparently this is an already known issue: ros-controls/gz_ros2_control#182

It appears that the main problem is the presence in the URDF of both position and effort command interfaces. @Hydran00 and I will try, in the next days, to propose a stable fix to this issue.

@caicaixia if you want to simulate (and use the robot) using only position control, you may safely delete all the effort command interfaces in the z1_description/urdf/z1.ros2_control.xacro file, and everything shall work out of the box.

[caicaixia]

Hello, good to see big progress on it. Thanks a lot.
I will try it.

[caicaixia]

Hello, I tried running the demo code as described in your README. I removed all the effort commands, and the robot is now able to move in the simulation.

However, the robot's motion seems a bit strange. After the initial movement, only Joint 1 continues to move during subsequent motions, while the other joints remain stationary.

1. ros2 launch z1_bringup z1.launch.py starting_controller:=joint_trajectory_controller
ros2 run z1_examples waypoint_test.py

z1_ros2_waypoints.mp4

2. I also tried the moveit. It seems like the robot is planned but not moving.
   ros2 launch z1_moveit z1_moveit.launch.py

Sorry, moveit demo is working now.

[Hydran00]

Hi @caicaixia,
this is a well-known problem related to how gazebo's hardware interfaces connect to controllers. There are open issues here and here.
What you can try to make it work is to comment all the torque command interfaces in the z1_description/urdf/z1.ros2_control.xacro for each joint. I am referring to these tags:

z1_ros2/z1_description/urdf/z1.ros2_control.xacro

Lines 40 to 43 in 78e9009

	<command_interface name="effort">
	<param name="min"> -${torqueMax} </param>
	<param name="max"> ${torqueMax} </param>
	</command_interface>

and then re-build the workspace.

@matteodv99tn maybe it would be great to mention this problem in the README?

EDIT:
sorry, you already did it 😂

[caicaixia]

Thanks, yes. I already saw that problem and commented all the torque comments. But there are still has some other problems here. Can you check my videos. Thanks

[Hydran00]

I am sorry, I misunderstood your problem. Anyway, I managed to replicate your issue. I suppose there is a problem of self-collision in the simulation when every joint is set to zero. I tried sending different trajectories here, avoiding the all-zero configuration and I had no problems:

z1_ros2/z1_examples/z1_examples/waypoint_test.py

Lines 70 to 75 in 78e9009

	future = node.send_traj_request([
	np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
	np.array([0.0, 1.0, -1.0, 0.0, 0.0, 0.0]),
	np.array([np.pi / 4, 1.0, -1.0, 0.0, 0.0, 0.0]),
	np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
	])

Let me know if this helps.

[caicaixia]

@Hydran00 Thanks for your help and quick response. Now it's working! 👍

[caicaixia]

Sorry to bother you again. Today, I tried running z1_ros2 on the real robot, but I encountered an error related to accessing the z1_controller.

I’ve carefully read the Z1 Hardware Interface README, but I’m still not sure what the issue is.

---

Update solution:
I have update your CMakeLists.txt to install z1_ctrl as an executable, like this:

install(
    PROGRAMS ${CMAKE_CURRENT_BINARY_DIR}/z1_ctrl
    DESTINATION share/${PROJECT_NAME}/controller
)

✅ PROGRAMS tells CMake: "This is an executable file — preserve execute permissions."

[caicaixia]

---

I commented out the following line in the waypoint_test.py, and now the robot is able to move:

# np.array([0.0, 0.02, -0.02, 0.0, 0.0, 0.0]),
np.array([0.1, 1.0, -1.0, 0.0, 0.0, 0.0]),
np.array([np.pi / 4, 1.0, -1.0, 0.0, 0.0, 0.0]),
np.array([0.0, 0.02, -0.05, 0.0, 0.0, 0.0]),

Current I can move the robot both using the bring_up and the moveIt. However, the robot's motion is very jerky and not smooth.

[matteodv99tn]

Hi @caicaixia , thank you for providing the correction to the CMakeLists.txt to properly preserve the executable permissions. I just included the fix on the main branch so that everyone can benefit from this!

Concerning the jerkiness of the robot motion, are you considering the action on the real robot? Because we observed jerky movement from the robot side, but concluded that the high friction at the joint-level makes precise robot movement hard to achieve, and the low-level controller struggles in following the desired motion.