Add support for controlling Z1 arm mounted on Aliengo quadruped (aliengoZ1 model)

[paulblum]

This pull request addresses issue #6 by extending support for controlling the Unitree Z1 robotic arm when it is mounted on a quadruped; e.g., using the aliengoZ1_description model in unitree_ros.

Issue:

Failed attempt to control Z1 arm joints within aliengoZ1_description in Gazebo

Currently, the z1_controller repository provides a ROS control interface for the Unitree Z1 robotic arm in the /z1_gazebo namespace:

Relevant ROS nodes and topics during operation of z1_description in Gazebo by z1_controller

However, there is a need to extend support for controlling the Z1 arm when it is mounted on a quadruped model with a different namespace. For example, using the aliengoZ1_description model where the namespace is /aliengoZ1_gazebo:

Relevant ROS nodes and topics during attempted operation of Z1 arm within aliengoZ1_description in Gazebo by z1_controller; note dead and leaf topics due to namespace mismatch

The issue stems from a hard-coding of the /z1_gazebo namespace within the Z1 control interface for ROS Gazebo simulation, IOROS:

z1_controller/sim/IOROS.cpp

Lines 67 to 85 in 8eb6595

	void IOROS::_initSend(){
	_servo_pub[0] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint01_controller/command", 1);
	_servo_pub[1] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint02_controller/command", 1);
	_servo_pub[2] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint03_controller/command", 1);
	_servo_pub[3] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint04_controller/command", 1);
	_servo_pub[4] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint05_controller/command", 1);
	_servo_pub[5] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint06_controller/command", 1);
	_servo_pub[6] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/gripper_controller/command", 1);
	}
	void IOROS::_initRecv(){
	_servo_sub[0] = _nm.subscribe("/z1_gazebo/Joint01_controller/state", 1, &IOROS::_joint00Callback, this);
	_servo_sub[1] = _nm.subscribe("/z1_gazebo/Joint02_controller/state", 1, &IOROS::_joint01Callback, this);
	_servo_sub[2] = _nm.subscribe("/z1_gazebo/Joint03_controller/state", 1, &IOROS::_joint02Callback, this);
	_servo_sub[3] = _nm.subscribe("/z1_gazebo/Joint04_controller/state", 1, &IOROS::_joint03Callback, this);
	_servo_sub[4] = _nm.subscribe("/z1_gazebo/Joint05_controller/state", 1, &IOROS::_joint04Callback, this);
	_servo_sub[5] = _nm.subscribe("/z1_gazebo/Joint06_controller/state", 1, &IOROS::_joint05Callback, this);
	_servo_sub[6] = _nm.subscribe("/z1_gazebo/gripper_controller/state", 1, &IOROS::_gripperCallback, this);
	}

Proposed Changes:

Implement a mechanism to automatically detect the correct namespace based on the model being used when z1_controller begins execution, and use it to configure the namespace of ROS topics for Z1 joint control.

Insights:

• Unitree's quadruped models (e.g., aliengoZ1_description) initialize a ROS parameter called /robot_name in their launch files, each specifying a model's unique namespace.
• The /robot_name parameter can be retrieved and used to configure the IOROS interface with the correct namespace for Z1 joints.
  • Similar to Unitree's implementations in servo.cpp and move_publisher.cpp.
• Note that z1.launch does not initialize a /robot_name parameter; the original /z1_gazebo namespace should be used in this case.

Implementation details:

• Added a private member variable _rname of type std::string to the IOROS class.
  • During IOROS initialization, _rname stores /robot_name retrieved from the ROS parameter server, defaulting to "z1" if no such parameter is found.
• _rname configures the namespace for Z1 joints whenever IOROS::_initSend() or IOROS::_initRecv() are invoked.

Results:

The modification maintains z1_controller's functionality for control of z1_description in Gazebo, while enabling the capability of z1_controller to control the Z1 arm when it exists in a different robot namespace:

Relevant ROS nodes and topics during operation of z1_description in Gazebo by updated z1_controller	Relevant ROS nodes and topics during successful operation of Z1 arm within aliengoZ1_description in Gazebo by updated z1_controller

Update enables simultaneous control of quadruped and Z1 arm joints in Gazebo; unitree_servo and z1_controller's demo trajectory visualized here