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 all 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
47 changes: 47 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,14 @@ 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.handle_arm_velocity_command,
1,
callback_group=self.group,
)
self.create_service(
Trigger,
"claim",
Expand Down Expand Up @@ -2919,6 +2929,43 @@ def handle_set_gripper_camera_parameters(

return response

def handle_arm_velocity_command(self, arm_velocity_command: ArmVelocityCommandRequest) -> None:
if self.spot_wrapper is None:
return

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

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

cartesian_velocity = arm_command_pb2.ArmVelocityCommand.CartesianVelocity()
cartesian_velocity.velocity_in_frame_name.x = (
arm_velocity_command.command.cartesian_velocity.velocity_in_frame_name.x
)
cartesian_velocity.velocity_in_frame_name.y = (
arm_velocity_command.command.cartesian_velocity.velocity_in_frame_name.y
)
cartesian_velocity.velocity_in_frame_name.z = (
arm_velocity_command.command.cartesian_velocity.velocity_in_frame_name.z
)
cartesian_velocity.frame_name = arm_velocity_command.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,
)
Comment on lines +2936 to +2965
Copy link
Collaborator

@khughes-bdai khughes-bdai May 21, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

You should be able to replace this with the following, using some built in proto to ros conversion:

Suggested change
cylindrical_velocity = arm_command_pb2.ArmVelocityCommand.CylindricalVelocity()
cylindrical_velocity.linear_velocity.r = arm_velocity_command.command.cylindrical_velocity.linear_velocity.r
cylindrical_velocity.linear_velocity.theta = (
arm_velocity_command.command.cylindrical_velocity.linear_velocity.theta
)
cylindrical_velocity.linear_velocity.z = arm_velocity_command.command.cylindrical_velocity.linear_velocity.z
angular_velocity = geometry_pb2.Vec3(
x=arm_velocity_command.angular_velocity_of_hand_rt_odom_in_hand.x,
y=arm_velocity_command.angular_velocity_of_hand_rt_odom_in_hand.y,
z=arm_velocity_command.angular_velocity_of_hand_rt_odom_in_hand.z,
)
cartesian_velocity = arm_command_pb2.ArmVelocityCommand.CartesianVelocity()
cartesian_velocity.velocity_in_frame_name.x = (
arm_velocity_command.command.cartesian_velocity.velocity_in_frame_name.x
)
cartesian_velocity.velocity_in_frame_name.y = (
arm_velocity_command.command.cartesian_velocity.velocity_in_frame_name.y
)
cartesian_velocity.velocity_in_frame_name.z = (
arm_velocity_command.command.cartesian_velocity.velocity_in_frame_name.z
)
cartesian_velocity.frame_name = arm_velocity_command.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,
)
proto_command = arm_command_pb2.ArmVelocityCommand.Request()
convert(arm_velocity_command, proto_command)

Can you verify that this still works with your example?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Will test it later this afternoon on the robot :)

Copy link
Collaborator

Choose a reason for hiding this comment

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

Hello, just wondering if you had a chance to test this out?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Hey, sorry didn't had the time yet and our robot is not available for the next two weeks. I will test it then


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