This is a ROS 2 package designed to communicate with Spot's joint control API through ROS 2 control.
It uses the hardware interface provided in the spot_hardware_interface package to connect to the robot (and also supports a mock hardware interface that forwards commands directly to state).
By default, it loads a standard joint state broadcaster and forward position controller provided from ros2_control.
Alternative controllers from spot_controllers that forwards position, velocity, effort, and gain commands at the same time are also available.
If your parameters for authenticating with your robot are set in the environment variables BOSDYN_CLIENT_USERNAME, BOSDYN_CLIENT_PASSWORD, and SPOT_IP, run:
ros2 launch spot_ros2_control spot_ros2_control.launch.py hardware_interface:=robotImportant
When taking control of Spot with the tablet, make sure to release control back, or spot_ros2_control will not be able to command the robot.
Login information can also be set in a configuration file, the same as in the spot_driver launchfile.
For example, if you have a config file spot_ros.yaml containing the following information:
/**:
ros__parameters:
username: "username"
password: "password"
hostname: "00.00.00.00"
You can then run the launchfile with the following command:
ros2 launch spot_ros2_control spot_ros2_control.launch.py hardware_interface:=robot config_file:=path/to/spot_ros.yamlIf you wish to launch these nodes in a namespace, add the argument spot_name:=<Robot Name> (note that this will also prefix the joints).
This hardware interface will stream the joint angles of the robot at 333 Hz onto the topic /<Robot Name>/low_level/joint_states.
By default, the ros2 control stack will launch using forward_position_controller.
This uses a built-in ROS 2 controller for streaming joint level commands on the topic /<Robot Name>/forward_position_controller/commands.
This will forward position commands directly to the joint control API through the hardware interface.
The controller expects the command array to contain the list of positions to forward for each joint on the robot (12 elements for robots without an arm, and 19 for robots with an arm).
An alternative feed-forward controller provided by the spot_controllers package can be used to specify the position, velocity, and effort of all joints at the same time.
To bring up this controller, add the launch argument robot_controllers:=forward_state_controller.
Commands can then be sent on the topic /<Robot Name>/forward_state_controller/commands.
This controller expects the ordering of the command array to be [<positions for each joint>, <velocities for each joint>, <efforts for each joint>] (36 elements for robots without an arm, and 57 for robots with an arm).
Finally, the custom controller spot_joint_controller is also provided for the ability to stream position, velocity, effort, k_q_p, and k_qd_p for any subset of joints.
To enable this controller, add the launch argument robot_controllers:=spot_joint_controller.
This controller accepts commands on the topic /<Robot Name>/spot_joint_controller/joint_commands.
Unlike the previous controllers, you do not need to set a value for every single joint, but instead can select the joints by name you wish to control in the joint_commands message -- all other joints that are not specified will be left unchanged.
More details on this controller can be found on the spot_controllers README.
Caution
When using these available controllers, there is no safety mechanism in place to ensure smooth motion. Specifically, for the forward position and state controllers, the ordering of the command must match the ordering of the joints specified in the controller configuration file (here for robots with an arm or here for robots without an arm), and the robot can move in unpredictable and dangerous ways if this is not set correctly. Make sure to keep a safe distance from the robot when working with these controllers and ensure the e-stop can easily be pressed if needed.
IMU data from the robot state stream is exposed on the topic /<Robot Name>/imu_sensor_broadcaster/imu.
Feet contact data from the robot state stream is exposed on the topic /<Robot Name>/foot_state_broadcaster/feet.
Estimates of the robot's body pose with respect to vision and odom frames can be found via the TF frames <Robot Name>/low_level/vision and <Robot Name>/low_level/odom, or alternatively on the topics /<Robot Name>/spot_pose_broadcaster/vision_t_body and /<Robot Name>/spot_pose_broadcaster/odom_t_body.
Additionally, the state publisher node, object synchronization node, and image publisher nodes from spot_driver will get launched by default when running on the robot to provide extra information such as additional TF frames, odometry, and camera feeds.
To turn off the image publishers (which can cause problems with bandwidth), add the launch argument launch_image_publishers:=false.
As mentioned above, joint level gains can be streamed to the robot while using spot_joint_controller.
If you are using a different controller, but still want to set the initial value of each of these gains, joint level gains can also be specified in the config_file under the parameter names k_q_p and k_qd_p.
If you do not specify these parameters, the default gains from the spot-sdk joint control examples are used during command streaming.
The example below shows a valid configuration for a robot with an arm as each list contains 19 elements.
More information on how these gains are used by Spot can be found here.
/**:
ros__parameters:
k_q_p: [624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0]
k_qd_p: [5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32]
To use a mock hardware interface, run:
ros2 launch spot_ros2_control spot_ros2_control.launch.py hardware_interface:=mockBy default, this will load a robot with no arm. If you want your mock robot to have an arm, add the launch argument mock_arm:=True.
controllers_config: If this argument is unset, a general purpose controller configuration will be loaded containing a forward position controller and a joint state publisher, that is filled appropriately based on whether or not the robot used (mock or real) has an arm. The forward state controller and spot joint controller are also specified here. If you wish to load different controllers, this can be set here.robot_controllers: These are the names of the robot controllers that will be started when the launchfile is called. The default is the simple forward position controller. Each name must match a controller in thecontrollers_configfile.launch_rviz: If you do not want rviz to be launched, add the argumentlaunch_rviz:=False.auto_start: If you do not want hardware interfaces and controllers to be activated on launch, add the argumentauto_start:=False.
Examples are provided to replicate these joint control examples from Boston Dynamics.
They are designed to show how you can send commands to the robot using the built in forward_position_controller, and can be run in mock mode or on the real robot.
Run the following commands to test these examples after launching spot_ros2_control.launch.py. If your robot does not have an arm:
ros2 launch spot_ros2_control noarm_squat.launch.pyOr, if your robot does have an arm:
ros2 launch spot_ros2_control wiggle_arm.launch.pyAdd the launch argument spot_name:=<namespace> if the ros2 control stack was launched in a namespace.
An alternate example using the spot_joint_controller is provided to demonstrate how to stream position and gains at the same time.
First launch spot_ros2_control.launch.py with the launch argument robot_controllers:=spot_joint_controller.
Next, run
ros2 run spot_ros2_control set_grippper_gainsInclude the argument --robot <namespace> if the ros2 control stack was launched in a namespace.
This demo will repeatedly open and close the gripper, and after each motion, will take user input on new k_q_p and k_qd_p values to use next.
An example demonstrating how to switch between low level and high level control is also provided -- more details about the unique setup this requires can be found in the following section.
It is possible to use both the joint level control and traditional high level control (as used by spot_driver).
To do this, launch the spot driver with the controllable flag:
ros2 launch spot_driver spot_driver.launch.py controllable:=True <other launch args>Tip
A simple Python example demonstrating how to switch between these two modes (assuming the driver is run with the controllable flag) can be found in examples/mixed_level_example.py.
You can run it with ros2 run spot_ros2_control mixed_level_example with the optional --robot <namespace> argument if the driver was launched in a namespace.
Note that you will have to release control of the robot on the tablet before running in order for this example to work.
Launching the driver in this way will bring up the nodes in spot_driver as usual, but also bring up ROS 2 control stack for Spot side by side in the unconfigured state.
At this point, you can use the spot_driver interfaces as usual, such as the claim, power_on, stand, services, or the robot_command action for more complicated commands, but you cannot interact with any of the joint level commands from ROS 2 control yet as none of the controllers are active.
To enable joint level control, activate the hardware interface, load and configure the controller(s), and activate the controller(s) using service calls to the controller manager (specifically to set_hardware_component_state, load_controller, and switch_controller).
Once the hardware interface and the chosen controllers are activated, you can interact with them as normal -- for example, a subscriber can fetch the joint states from the joint_state_broadcaster, and you can use a publisher to forward joint commands to the robot.
We have provided simple forwarding controllers as examples in spot_controllers but other custom controllers can also be used provided the config file .
It is important to note that once these controllers have been activated, you will not be able to interact with the spot_driver provided interfaces anymore, as the hardware interface owns the Spot's lease.
To switch back to "high level" mode, use the same controller manager services to deactivate and unload the controllers and set the hardware interface back to unconfigured (specifically by calling switch_controllers, unload_controller, and set_hardware_component_state).
After this, you can go back to using the standard ROS interfaces to communicate with spot_driver nodes as it once again has control of the Spot lease.