Skip to content

Commit

Permalink
Added possibility to directly control the arm using an ArmVelocityCom…
Browse files Browse the repository at this point in the history
…mand (#104)

## Change Overview

Added the ability to set a Velocity command for the arm. This enables a user to have direct translational and rotational  control of the TCP movement using a controller or any other input device. The corresponding [PR](bdaiinstitute/spot_ros2#366) on the driver Repo provides a topic interface to send the corresponding movement command. 
The angular and Cartesian velocity were thoroughly tested during operation on our spot. For the cylindrical velocity I performed some basic tests but didn't evaluate over hours as the other two input methods since I don't really use them in this reference frame.

## Testing Done
    - [x] tested that these new functionality work on robot
  • Loading branch information
MGBla authored Jun 5, 2024
1 parent 8aca7b7 commit 67573db
Showing 1 changed file with 44 additions and 0 deletions.
44 changes: 44 additions & 0 deletions spot_wrapper/spot_arm.py
Original file line number Diff line number Diff line change
Expand Up @@ -495,6 +495,50 @@ def hand_pose(self, data) -> typing.Tuple[bool, str]:

return True, "Moved arm successfully"

def handle_arm_velocity(
self, arm_velocity_command: arm_command_pb2.ArmVelocityCommand.Request, cmd_duration: float = 0.15
) -> typing.Tuple[bool, str]:
"""
Set the velocity of the arm TCP
Args:
arm_velocity_command: Protobuf message to set the arm velocity
cmd_duration: (optional) Time-to-live for the command in seconds.
Returns:
Boolean success, string message
"""

try:
success, msg = self.ensure_arm_power_and_stand()
if not success:
self._logger.info(msg)
return False, msg
else:
end_time = self._robot.time_sync.robot_timestamp_from_local_secs(time.time() + cmd_duration)

arm_velocity_command2 = arm_command_pb2.ArmVelocityCommand.Request(
cylindrical_velocity=arm_velocity_command.cylindrical_velocity,
angular_velocity_of_hand_rt_odom_in_hand=arm_velocity_command.angular_velocity_of_hand_rt_odom_in_hand,
cartesian_velocity=arm_velocity_command.cartesian_velocity,
maximum_acceleration = arm_velocity_command.maximum_acceleration,
end_time=end_time,
)

robot_command = robot_command_pb2.RobotCommand()
robot_command.synchronized_command.arm_command.arm_velocity_command.CopyFrom(arm_velocity_command2)

self._robot_command_client.robot_command(
command=robot_command, end_time_secs=time.time() + cmd_duration
)

except Exception as e:
return (
False,
f"An error occured while trying to move arm\n Exception: {e}",
)
return True, "Moved arm successfully"

@staticmethod
def block_until_gripper_command_completes(
robot_command_client: RobotCommandClient,
Expand Down

0 comments on commit 67573db

Please sign in to comment.