Failed to send command when using ros2 control

[jacobbreen25]

I am attempting to use the ros2 control interface to create a joint trajectory controller and use MoveIt to move the arm. In doing this, I keep getting the following error:

[ros2_control_node-1] [ERROR] [1736803777.858152464] [SpotHardware]: Failed to send command: '3(Streaming for joint control failed): '

I was also getting this error with running the default controllers.

It was working without this error earlier this morning

System Info:
ROS2 version: Humble
OS: Linux
Wifi: Private wifi (Not the robot's local wifi)

[khughes-bdai]

Can you verify that your robot still has a valid joint level API license? Assuming you've tried rebooting it too.
Due to limitations of the Spot C++ SDK it is very difficult to get a description of why this error is happening. (It exists in their Python API for joint level control but does not appear to have been carried to C++...). One reason it could be happening is your network connection to the robot is not stable. If you are connecting to the robot over wifi, you will get better behavior over Ethernet. If the connection is bad, the joint level commands could also be expiring before they reach the robot. The default "time to live" of the joint command is 50ms and the extrapolation time is 5 ms. You can locally try increasing this to see if it makes a difference?

[khughes-bdai]

another thing I tried when debugging this issue a while back was running one of BD's examples to see if it was a robot problem or a ROS problem https://github.com/boston-dynamics/spot-cpp-sdk/blob/master/cpp/examples/joint_control/wiggle_arm_example.cpp

[jacobbreen25]

I installed the joint level API license a few days ago so it should not be that. It looks like my spot-cpp-sdk did not install the examples when installed from the spot_ros2 install bash (or at least I cannot find them) so I was not able to run the wiggle arm example except for the example that could be run from ROS (This defeats the purpose of us trying it in the first place but I do not think this is a ROS2 issue given that the error comes up from the message status returned from the robot). I tried bringing the time to live up to 1000 ms (Extremely high to ensure it is not the problem) and the extrapolation time up to 100 ns and it is still happening. We may try changing the wifi being used by the robot to see if this makes a difference although the ping speed was rather fast (Could still be a firewall issue)

[khughes-bdai]

Trying a different wifi sounds promising. You could also try their same examples with the python sdk which are simpler to set up. If this doesn't work out of the box I'd recommend reaching out to BD support

[jacobbreen25]

Sorry for the late response, I have been rather busy with other projects.

I tried using the robots local wifi and get the same error ([ros2_control_node-1] [ERROR] [1737740713.114368181] [SpotHardware]: Failed to send command: '3(Streaming for joint control failed): ')

I then tried to run the wiggle_arm.py example you sent and when trying this, I got the following exception:

2025-01-24 13:16:20,498 - ERROR - Hello, Spot! threw an exception: ResourceAlreadyClaimedError()
Traceback (most recent call last):
  File "/usr/local/lib/python3.10/dist-packages/bosdyn/client/lease.py", line 466, in _get_lease_state_locked
    return self._lease_state_map[resource]
KeyError: 'body'

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "/usr/local/lib/python3.10/dist-packages/bosdyn/client/lease.py", line 901, in __init__
    self._lease_wallet.get_lease(self._resource)
  File "/usr/local/lib/python3.10/dist-packages/bosdyn/client/lease.py", line 438, in get_lease
    return self._get_owned_lease_state_locked(resource).lease_current
  File "/usr/local/lib/python3.10/dist-packages/bosdyn/client/lease.py", line 481, in _get_owned_lease_state_locked
    lease_state = self._get_lease_state_locked(resource)
  File "/usr/local/lib/python3.10/dist-packages/bosdyn/client/lease.py", line 468, in _get_lease_state_locked
    raise NoSuchLease(resource)
bosdyn.client.lease.NoSuchLease: No lease for resource "body"

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "/home/jbreen/warehouse_ws/wiggle_arm.py", line 151, in main
    run_joint_control(options)
  File "/home/jbreen/warehouse_ws/wiggle_arm.py", line 70, in run_joint_control
    with bosdyn.client.lease.LeaseKeepAlive(lease_client, must_acquire=True, return_at_exit=True):
  File "/usr/local/lib/python3.10/dist-packages/bosdyn/client/lease.py", line 904, in __init__
    self._lease_client.acquire(self._resource)
  File "/usr/local/lib/python3.10/dist-packages/bosdyn/client/lease.py", line 537, in acquire
    return self.call(self._stub.AcquireLease, req, self._handle_acquire_success,
  File "/usr/local/lib/python3.10/dist-packages/bosdyn/client/common.py", line 307, in processor
    return func(self, rpc_method, request, value_from_response=value_from_response,
  File "/usr/local/lib/python3.10/dist-packages/bosdyn/client/common.py", line 449, in call
    return self.handle_response(response, error_from_response, value_from_response)
  File "/usr/local/lib/python3.10/dist-packages/bosdyn/client/common.py", line 457, in handle_response
    raise exc  # pylint: disable=raising-bad-type
bosdyn.client.lease.ResourceAlreadyClaimedError: bosdyn.api.AcquireLeaseResponse (ResourceAlreadyClaimedError): Use TakeLease method to forcefully grab the already claimed lease.

[jacobbreen25]

From Boston Dynamics:

Thanks for reaching out! I am sorry to let you know that Boston Dynamics does not support the AI Institute's ROS2 wrapper. From the stack trace posted in the issue you cited, it appears as though your program did not hold the lease when commanding Spot to move. It is possible that a tablet, other device, or other program was connected to Spot and held motor power, which prevented your program from taking control. You may forcefully take the lease using the LeaseClient's take method, or by ensuring no other program holds the lease.

I have tried releasing the lease manually on the controller but the problem still persists.

I was able to get the arm moving with the spot sdk.

[khughes-bdai]

I was able to get the arm moving with the spot sdk.

just to be clear, was this via successfully running their wiggle arm python example? or is that also still not working (with the error you posted in the stack trace above)?

[jacobbreen25]

I was able to get the wiggle_arm.py script to work. It turns out the tablet does not release the lease automatically, we had to do it manually.

[khughes-bdai]

And if you release the lease from the tablet, then, and try the ros2 control example, are you still in the same situation of Failed to send command: '3(Streaming for joint control failed)?

[jacobbreen25]

Yes, it is still running this error after manually releasing the lease from the tablet.

Note that our robot software version is 4.1.0 (I forgot to mention this on the original issue)

[khughes-bdai]

Hmm, I don't have a great immediate solution to this, especially given that it was working initially on your robot? There is no other error message from the hardware interface at launch?
Two ideas:
We recently updated this repo to 4.1.1, you could try installing the latest main and re-running (although this probably won't make a difference as there are no updates to the joint level api according to BD's changelog).
You can also try BD's C++ API outside of ROS by just making the example C++ executable locally and linking it to your installed version of spot-cpp-sdk. (I attached a super hacky way that I have successfully tested this in the past if it's useful: test-low-level.zip). it's possible that there are some differences between the python and C++ sdk, as C++ is still considered a beta release

[jacobbreen25]

It looks like the ROS2 control version of this still failing even though we updated the robot. I also built the c++ example you provided and it IS working.