Kabirk/add arm wasd by kabirkedia · Pull Request #734 · bdaiinstitute/spot_ros2

kabirkedia · 2025-09-04T17:37:24Z

Change Overview

Adds better arm control to the driver.

Testing Done

Please create a checklist of tests you plan to do and check off the ones that have been completed successfully. Ensure that ROS 2 tests use domain_coordinator to prevent port conflicts. Further guidance for testing can be found on the ros utilities wiki.

tcappellari-bdai · 2025-09-04T19:28:15Z

When this is ready for review, I'll find some time to test it on our robot. Thanks!

tcappellari-bdai · 2025-09-04T19:38:27Z

When this is ready for review, I'll find some time to test it on our robot. Thanks!

ah, I just saw the related spot_wrapper PR. I'll find some time to review and test them next week

kabirkedia · 2025-09-04T19:45:45Z

Hey @tcappellari-bdai,This should be ready for review. I just need your opinion onon bdaiinstitute/spot_wrapper#179, specifically regarding the arm velocity control. Do you think the end time should be set by the user or handled internally by the spot_ros2 driver?
If we go with the driver handling it, we could either introduce a custom message for arm velocity or reuse Twist.

spot_examples/spot_examples/wasd.py

spot_examples/docs/wasd.md

spot_examples/spot_examples/arm_wasd.py

khughes-bdai · 2025-10-02T21:03:29Z

Left some other comments on bdaiinstitute/spot_wrapper#179, but I think this example will be super useful to have in soon. Thanks @kabirkedia!

kabirkedia · 2025-10-10T14:26:03Z

@khughes-bdai this is ready to merge
Let me know if any changes required

khughes-bdai · 2025-10-20T21:33:51Z

Hi @kabirkedia! Just tested this out on robot, sorry for the super long delay. The demo is working great! I also tried out just publishing to cmd_vel in the terminal and verified that was working the same as before. Two things I ran into:

when teleoperating the arm with arm_wasd I kept getting this error spammed in the terminal, not sure what is causing it. The robot arm was still moving fine despite this printout. Will look into it on the side but let me know if you have any ideas or if you have also ran into this

[spot_ros2-1] [ERROR] [1760995020.400281894] [spot_ros2]: Failed to execute arm velocity command: An error occured while trying to move arm
[spot_ros2-1]  Exception: bosdyn.api.RobotCommandResponse (InvalidRequestError): Received request with unset command.
[spot_ros2-1] [ERROR] [1760995020.771800650] [spot_ros2]: Failed to execute arm velocity command: An error occured while trying to move arm
[spot_ros2-1]  Exception: bosdyn.api.RobotCommandResponse (InvalidRequestError): Received request with unset command.
...

When in arm teleoperation mode, and then switching to walking mode or sitting, the arm tries to stay at the same place in the world frame. This left the arm in a weird spot when I shut down the example. I think it would be useful on shut down to stow the arm, and then possibly when switching between arm + body mode to lock the arm in body instead of in world. What do you think?

Also, can you run pre-commit install && pre-commit run --all and re-push, I think there are a few small linting errors. Can you also revert the changes to spot_wrapper on this PR? Your associated PR in spot_wrapper has already been brought into spot_ros2 with dependabot.
Thanks!!

khughes-bdai · 2025-10-21T13:44:30Z

spot_examples/spot_examples/arm_wasd.py

+        # self.logger.info("Standing robot up")
+        # result = self.robot.command("stand")
+        # if not result.success:
+        #     self.logger.error("Robot did not stand message was " + result.message)
+        #     return False


can this be deleted/uncommented?

khughes-bdai · 2025-10-21T15:10:50Z

spot_examples/spot_examples/arm_wasd.py

+        while time.time() - start_time < ARM_VELOCITY_CMD_DURATION:
+            self.pub_arm_vel.publish(arm_cmd)
+            time.sleep(0.01)
+        self.pub_arm_vel.publish(ArmVelocityCommandRequest())


I think this might be the cause of the error I was seeing in the terminal:

[spot_ros2-1] [ERROR] [1760995022.039689202] [spot_ros2]: Failed to execute arm velocity command: An error occured while trying to move arm [spot_ros2-1] Exception: bosdyn.api.RobotCommandResponse (InvalidRequestError): Received request with unset command.

This is because ArmVelocityRequest() instead of being a zero command is read as an uninitialized command. I think you will have to explicitly define this as a proto first, then convert to the ros message form. This is just a little more robust than filling in the ros message manually and ensures all of the appropriate fields are captured. I think it would be useful to do this both for the command from the user input as well as the zero command at the end

from bosdyn.api import arm_command_pb2 arm_cmd_request = arm_command_pb2.ArmVelocityCommand.Request() # then fill in the attributes the same way you are doing above arm_cmd_request.angular_velocity_of_hand_rt_odom_in_hand.x = end_effector_x arm_cmd_request.angular_velocity_of_hand_rt_odom_in_hand.y = end_effector_y ... arm_cmd_request_ros = ArmVelocityCommandRequest() convert(arm_cmd_request, arm_cmd_request_ros) # then publish arm_cmd_request_ros

your zero command at the end would be

zero_arm_cmd_request = arm_command_pb2.ArmVelocityCommand.Request() zero_arm_cmd_request_ros = ArmVelocityCommandRequest() convert(zero_arm_cmd_request, zero_arm_cmd_request_ros)

khughes-bdai · 2025-10-21T15:11:39Z

spot_examples/spot_examples/arm_wasd.py

+    # try:
+    os.environ.setdefault("ESCDELAY", "0")
+    curses.wrapper(wasd_interface.drive)
+    # except Exception as e:
+    #     wasd_interface.logger.error("WASD has thrown an error:" + str(e))


can these comments be removed?

spot_examples/spot_examples/arm_wasd.py

Co-authored-by: Katie Hughes <157421702+khughes-bdai@users.noreply.github.com>

kabirkedia · 2025-11-12T00:30:44Z

Hey @khughes-bdai ,
Sorry for the delay from my side but I did not get time to work with the spot recently due to shift in project changes. I just made all the necessary changes
Please look into them and let me know!

kabirkedia · 2025-11-12T00:44:06Z

Hi @kabirkedia! Just tested this out on robot, sorry for the super long delay. The demo is working great! I also tried out just publishing to cmd_vel in the terminal and verified that was working the same as before. Two things I ran into:

when teleoperating the arm with arm_wasd I kept getting this error spammed in the terminal, not sure what is causing it. The robot arm was still moving fine despite this printout. Will look into it on the side but let me know if you have any ideas or if you have also ran into this
[spot_ros2-1] [ERROR] [1760995020.400281894] [spot_ros2]: Failed to execute arm velocity command: An error occured while trying to move arm
[spot_ros2-1]  Exception: bosdyn.api.RobotCommandResponse (InvalidRequestError): Received request with unset command.
[spot_ros2-1] [ERROR] [1760995020.771800650] [spot_ros2]: Failed to execute arm velocity command: An error occured while trying to move arm
[spot_ros2-1]  Exception: bosdyn.api.RobotCommandResponse (InvalidRequestError): Received request with unset command.
...
When in arm teleoperation mode, and then switching to walking mode or sitting, the arm tries to stay at the same place in the world frame. This left the arm in a weird spot when I shut down the example. I think it would be useful on shut down to stow the arm, and then possibly when switching between arm + body mode to lock the arm in body instead of in world. What do you think?

Also, can you run pre-commit install && pre-commit run --all and re-push, I think there are a few small linting errors. Can you also revert the changes to spot_wrapper on this PR? Your associated PR in spot_wrapper has already been brought into spot_ros2 with dependabot. Thanks!!

Yes the arm locking in the world frame is a weird thing but I implemented a lock arm in place just for that. It should now lock the arm in place using joint commands.

def _velocity_cmd_helper(self, desc: str = "", v_x: float = 0.0, v_y: float = 0.0, v_rot: float = 0.0) -> None:
        if self.service_call_in_progress:
            self.add_message("Service call in progress, cannot send velocity command")
            return
        if self.is_arm_unstowed:
            if not self.is_arm_locked_in_position:
                self.service_call_in_progress = True
                result = self.lock_arm_in_position()
                self.service_call_in_progress = False
                if not result.success:
                    self.add_message("Failed to lock arm in position, cannot send velocity command")
                    self.is_arm_locked_in_position = False
                    return
                else:
                    self.is_arm_locked_in_position = True
        else:
            self.is_arm_locked_in_position = False
        twist = Twist()
        twist.linear.x = v_x
        twist.linear.y = v_y
        twist.angular.z = v_rot
        start_time = time.time()
        while time.time() - start_time < VELOCITY_CMD_DURATION:
            self.pub_cmd_vel.publish(twist)
            time.sleep(0.01)
        self.pub_cmd_vel.publish(Twist())

khughes-bdai · 2025-12-01T20:10:27Z

Hi @kabirkedia , sorry for the delay on my end! I just re tested this on robot. There were a few things I had to tweak manually to get it running. ~~I attached the full git diff here (diff.txt) -- if you could apply this to your branch it was working really great and we should be good to merge!~~ Edit: The changes I had to make are in the next comment!
Overall summary of things I changed -

I think the change from manually filling out the ros message -> filling out the proto and then converting to a ros message introduced a few bugs just with field names
I removed the empty arm velocity command at the end of the arm_velocity_cmd_helper function as it was still giving me errors in the terminal, and removing it got rid of the spew without seeming to affect the robot's behavior. I believe this is because the main arm velocity commands that are sent to the robot have a short time to live, and once they expire the robot doesn't move anymore.
i moved the call to lock_arm_in_place out of the velocity_cmd function and into the toggle_control function. With the initial location I think the code encounters some race conditions where sometimes the lock arm happens before the robot is moved and sometimes it happens after, leading to some unstable behavior. In my opinion having this happen on the control switch simplifies this logic.

I am also still seeing merge conflicts from the git submodules on this PR. Could you untrack them from your branch? Thanks!

khughes-bdai

Moved the changes I had to make locally to review format to make it easier to parse!

khughes-bdai · 2025-12-04T17:59:29Z

spot_driver/spot_driver/spot_ros2.py

        if not self.spot_wrapper:
            self.get_logger().info(f"Mock mode, received command vel {data}")
            return
+        self.spot_wrapper.velocity_cmd(data.linear.x, data.linear.y, data.angular.z, self.cmd_duration)


Forgot to catch this earlier, but this will currently break because of the order of the args from this spot_wrapper function: https://github.com/bdaiinstitute/spot_wrapper/blob/8e6ff4e55817952f5201fe510204324d023825ae/spot_wrapper/wrapper.py#L1288

I would suggest explicitly passing the arg names here to make sure that this is parsed correctly

Suggested change

self.spot_wrapper.velocity_cmd(data.linear.x, data.linear.y, data.angular.z, self.cmd_duration)

self.spot_wrapper.velocity_cmd(v_x=data.linear.x, v_y=data.linear.y, v_rot=data.angular.z, cmd_duration=self.cmd_duration)

khughes-bdai · 2025-12-04T18:00:07Z

spot_driver/spot_driver/spot_ros2.py

+        timestamp = data.header.stamp.sec + data.header.stamp.nanosec * 1e-9
        self.spot_wrapper.velocity_cmd(
-            v_x=data.linear.x, v_y=data.linear.y, v_rot=data.angular.z, cmd_duration=self.cmd_duration
+            data.twist.linear.x, data.twist.linear.y, data.twist.angular.z, timestamp, self.cmd_duration


Same comment here about explicitly passing the args:

Suggested change

data.twist.linear.x, data.twist.linear.y, data.twist.angular.z, timestamp, self.cmd_duration

v_x=data.linear.x, v_y=data.linear.y, v_rot=data.angular.z, timestamp=timestamp, cmd_duration=self.cmd_duration

khughes-bdai · 2025-12-04T18:01:35Z

spot_examples/spot_examples/arm_wasd.py

+        arm_cmd_request.maximum_acceleration.data = ARM_MAXIMUM_ACCELERATION
+        arm_cmd_request.command.command_choice = arm_cmd_request.command.COMMAND_CYLINDRICAL_VELOCITY_SET
+        arm_cmd_request.command.cylindrical_velocity.linear_velocity.r = r
+        arm_cmd_request.command.cylindrical_velocity.linear_velocity.theta = theta
+        arm_cmd_request.command.cylindrical_velocity.linear_velocity.z = z
+        arm_cmd_request.command.cylindrical_velocity.max_linear_velocity.data = ARM_MAX_LINEAR_VELOCITY


Think this was left over from switching from ros msg -> proto

Suggested change

arm_cmd_request.maximum_acceleration.data = ARM_MAXIMUM_ACCELERATION

arm_cmd_request.command.command_choice = arm_cmd_request.command.COMMAND_CYLINDRICAL_VELOCITY_SET

arm_cmd_request.command.cylindrical_velocity.linear_velocity.r = r

arm_cmd_request.command.cylindrical_velocity.linear_velocity.theta = theta

arm_cmd_request.command.cylindrical_velocity.linear_velocity.z = z

arm_cmd_request.command.cylindrical_velocity.max_linear_velocity.data = ARM_MAX_LINEAR_VELOCITY

arm_cmd_request.maximum_acceleration.value = ARM_MAXIMUM_ACCELERATION

arm_cmd_request.cylindrical_velocity.linear_velocity.r = r

arm_cmd_request.cylindrical_velocity.linear_velocity.theta = theta

arm_cmd_request.cylindrical_velocity.linear_velocity.z = z

arm_cmd_request.cylindrical_velocity.max_linear_velocity.value = ARM_MAX_LINEAR_VELOCITY

khughes-bdai · 2025-12-04T18:03:25Z

spot_examples/spot_examples/arm_wasd.py

+            self.add_message("Switched to ARM CONTROL mode")
+        else:
+            self.control_mode = "base"
+            self.add_message("Switched to BASE MOVEMENT mode")


I think it is more robust to lock the arm in place when you switch modes:

Suggested change

self.add_message("Switched to BASE MOVEMENT mode")

self.add_message("Switched to BASE MOVEMENT mode")

self.lock_arm_in_position()

khughes-bdai · 2025-12-04T18:04:05Z

spot_examples/spot_examples/arm_wasd.py

+        zero_arm_cmd_request = arm_command_pb2.ArmVelocityCommand.Request()
+        zero_arm_cmd_request_ros = ArmVelocityCommandRequest()
+        convert(zero_arm_cmd_request, zero_arm_cmd_request_ros)
+
+        self.pub_arm_vel.publish(zero_arm_cmd_request_ros)


I think this part can be removed. It gets rid of terminal spew without affecting the performance. I would also reccomend setting the arm_is_unstowed bool to true here as this moves the arm

Suggested change

zero_arm_cmd_request = arm_command_pb2.ArmVelocityCommand.Request()

zero_arm_cmd_request_ros = ArmVelocityCommandRequest()

convert(zero_arm_cmd_request, zero_arm_cmd_request_ros)

self.pub_arm_vel.publish(zero_arm_cmd_request_ros)

self.is_arm_unstowed = True

khughes-bdai · 2025-12-04T18:05:56Z

spot_examples/spot_examples/arm_wasd.py

+        if self.service_call_in_progress:
+            self.add_message("Service call in progress, cannot send velocity command")
+            return
+        if self.is_arm_unstowed:
+            if not self.is_arm_locked_in_position:
+                self.service_call_in_progress = True
+                result = self.lock_arm_in_position()
+                self.service_call_in_progress = False
+                if not result:
+                    self.add_message("Failed to lock arm in position, cannot send velocity command")
+                    self.is_arm_locked_in_position = False
+                    return
+                else:
+                    self.is_arm_locked_in_position = True
+        else:
+            self.is_arm_locked_in_position = False


If we handle locking the arm when we switch between arm/base mode, this whole block can be deleted

Suggested change

if self.service_call_in_progress:

self.add_message("Service call in progress, cannot send velocity command")

return

if self.is_arm_unstowed:

if not self.is_arm_locked_in_position:

self.service_call_in_progress = True

result = self.lock_arm_in_position()

self.service_call_in_progress = False

if not result:

self.add_message("Failed to lock arm in position, cannot send velocity command")

self.is_arm_locked_in_position = False

return

else:

self.is_arm_locked_in_position = True

else:

self.is_arm_locked_in_position = False

khughes-bdai · 2025-12-04T18:07:19Z

spot_examples/spot_examples/arm_wasd.py

+        """Lock arm in current position - placeholder for actual implementation"""
+        # This would need to be implemented based on your robot's API


Can you update/remove this comment?

kabirkedia added 6 commits September 2, 2025 17:49

add arm wasd

73b8915

try something

b002b04

add QoS

d5cbf8e

pass timestamp directly from ROS

946a9d3

add documentation

cf68b2c

add documentation

b5c4f6a

kabirkedia requested review from METEORITENMAX, khughes-bdai, mhidalgo-rai and tcappellari-bdai as code owners September 4, 2025 17:37

kabirkedia marked this pull request as draft September 4, 2025 17:37

kabirkedia and others added 3 commits September 4, 2025 13:37

Merge branch 'main' into kabirk/add-arm-wasd

8939cb3

added twist stamped in addtion to twsit

b7cee65

added twist stamped in addtion to twsit

503b22f

Merge branch 'main' into kabirk/add-arm-wasd

610ba70

kabirkedia marked this pull request as ready for review September 5, 2025 20:37

khughes-bdai mentioned this pull request Oct 2, 2025

Added timesync endpoint for the arm velocity control bdaiinstitute/spot_wrapper#179

Merged

khughes-bdai reviewed Oct 2, 2025

View reviewed changes

spot_examples/spot_examples/wasd.py Show resolved Hide resolved

khughes-bdai reviewed Oct 2, 2025

View reviewed changes

spot_examples/docs/wasd.md Show resolved Hide resolved

khughes-bdai reviewed Oct 2, 2025

View reviewed changes

spot_examples/spot_examples/arm_wasd.py Outdated Show resolved Hide resolved

kabirkedia and others added 2 commits October 10, 2025 10:25

merge to main

9d388f8

Merge branch 'main' into kabirk/add-arm-wasd

0d40a50

kabirkedia added 3 commits October 10, 2025 10:28

revert to original

e702da0

merge with main

1155ef6

revert to original

19ba230

kabirkedia added 2 commits October 10, 2025 10:33

revert to original

19d7819

revert to original

ac537e3

kabirkedia requested a review from khughes-bdai October 10, 2025 14:35

Merge branch 'main' into kabirk/add-arm-wasd

0bf385e

khughes-bdai reviewed Oct 21, 2025

View reviewed changes

spot_examples/spot_examples/arm_wasd.py Show resolved Hide resolved

kabirkedia and others added 5 commits November 11, 2025 19:05

Update spot_examples/spot_examples/arm_wasd.py

dd62f7e

Co-authored-by: Katie Hughes <157421702+khughes-bdai@users.noreply.github.com>

dependency checks

e39d7e4

final suggestions hopefully

5a07347

dependency resolver

0b3897e

conclude merge

554419a

kabirkedia requested a review from khughes-bdai November 12, 2025 00:30

pre-commit checks

556e511

khughes-bdai reviewed Dec 4, 2025

View reviewed changes

	self.spot_wrapper.velocity_cmd(data.linear.x, data.linear.y, data.angular.z, self.cmd_duration)
	self.spot_wrapper.velocity_cmd(v_x=data.linear.x, v_y=data.linear.y, v_rot=data.angular.z, cmd_duration=self.cmd_duration)

	data.twist.linear.x, data.twist.linear.y, data.twist.angular.z, timestamp, self.cmd_duration
	v_x=data.linear.x, v_y=data.linear.y, v_rot=data.angular.z, timestamp=timestamp, cmd_duration=self.cmd_duration

	self.add_message("Switched to BASE MOVEMENT mode")
	self.add_message("Switched to BASE MOVEMENT mode")
	self.lock_arm_in_position()

		"""Lock arm in current position - placeholder for actual implementation"""
		# This would need to be implemented based on your robot's API

Conversation

kabirkedia commented Sep 4, 2025

Change Overview

Testing Done

Uh oh!

tcappellari-bdai commented Sep 4, 2025

Uh oh!

tcappellari-bdai commented Sep 4, 2025

Uh oh!

kabirkedia commented Sep 4, 2025

Uh oh!

Uh oh!

Uh oh!

Uh oh!

khughes-bdai commented Oct 2, 2025

Uh oh!

kabirkedia commented Oct 10, 2025

Uh oh!

khughes-bdai commented Oct 20, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

kabirkedia commented Nov 12, 2025

Uh oh!

kabirkedia commented Nov 12, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

khughes-bdai commented Dec 1, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

khughes-bdai left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

khughes-bdai commented Oct 20, 2025 •

edited

Loading

kabirkedia commented Nov 12, 2025 •

edited

Loading

khughes-bdai commented Dec 1, 2025 •

edited

Loading