Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fb set arm velocity #366

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Fb set arm velocity #366

Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
51ae3e3
Added interface to set ArmVelocityCommand over a topic
MGBla Apr 18, 2024
6b088ca
Code formatting due to pre-commit hook
MGBla Apr 26, 2024
2f45a70
Changed signature of set_arm_velocity_callback
MGBla May 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 35 additions & 0 deletions spot_driver/spot_driver/spot_ros2.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
SingleGoalMultipleActionServers,
)
from bosdyn.api import (
arm_command_pb2,
geometry_pb2,
gripper_camera_param_pb2,
image_pb2,
Expand All @@ -44,6 +45,7 @@
from bosdyn_msgs.conversions import convert
from bosdyn_msgs.msg import (
ArmCommandFeedback,
ArmVelocityCommandRequest,
Camera,
FullBodyCommandFeedback,
GripperCommandFeedback,
Expand Down Expand Up @@ -495,6 +497,10 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw

self.create_subscription(Twist, "cmd_vel", self.cmd_velocity_callback, 1, callback_group=self.group)
self.create_subscription(Pose, "body_pose", self.body_pose_callback, 1, callback_group=self.group)
if has_arm:
self.create_subscription(
ArmVelocityCommandRequest, "arm_velocity", self.set_arm_velocity_callback, 1, callback_group=self.group
)
khughes-bdai marked this conversation as resolved.
Show resolved Hide resolved
self.create_service(
Trigger,
"claim",
Expand Down Expand Up @@ -2919,6 +2925,35 @@ def handle_set_gripper_camera_parameters(

return response

def set_arm_velocity_callback(self, data: ArmVelocityCommandRequest) -> None:
khughes-bdai marked this conversation as resolved.
Show resolved Hide resolved
if self.spot_wrapper is None:
return

cylindrical_velocity = arm_command_pb2.ArmVelocityCommand.CylindricalVelocity()
cylindrical_velocity.linear_velocity.r = data.command.cylindrical_velocity.linear_velocity.r
cylindrical_velocity.linear_velocity.theta = data.command.cylindrical_velocity.linear_velocity.theta
cylindrical_velocity.linear_velocity.z = data.command.cylindrical_velocity.linear_velocity.z

angular_velocity = geometry_pb2.Vec3(
x=data.angular_velocity_of_hand_rt_odom_in_hand.x,
y=data.angular_velocity_of_hand_rt_odom_in_hand.y,
z=data.angular_velocity_of_hand_rt_odom_in_hand.z,
)

cartesian_velocity = arm_command_pb2.ArmVelocityCommand.CartesianVelocity()
cartesian_velocity.velocity_in_frame_name.x = data.command.cartesian_velocity.velocity_in_frame_name.x
cartesian_velocity.velocity_in_frame_name.y = data.command.cartesian_velocity.velocity_in_frame_name.y
cartesian_velocity.velocity_in_frame_name.z = data.command.cartesian_velocity.velocity_in_frame_name.z
cartesian_velocity.frame_name = data.command.cartesian_velocity.frame_name

proto_command = arm_command_pb2.ArmVelocityCommand.Request(
cylindrical_velocity=cylindrical_velocity,
angular_velocity_of_hand_rt_odom_in_hand=angular_velocity,
cartesian_velocity=cartesian_velocity,
)

return self.spot_wrapper.spot_arm.handle_arm_velocity(proto_command)
Copy link
Collaborator

@khughes-bdai khughes-bdai Jun 3, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
return self.spot_wrapper.spot_arm.handle_arm_velocity(proto_command)
return self.spot_wrapper.spot_arm.handle_arm_velocity(proto_command, self.cmd_duration)

If we want this to mimic the cmd_vel topic, we can use the same duration field as it. This also makes it possible to change the rate from the ROS side of things.


def populate_camera_static_transforms(self, image_data: image_pb2.Image) -> None:
"""Check data received from one of the image tasks and use the transform snapshot to extract the camera frame
transforms. This is the transforms from body->frontleft->frontleft_fisheye, for example. These transforms
Expand Down
Loading