-
Notifications
You must be signed in to change notification settings - Fork 0
Quest Teleop PR#1 #962
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.
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
base: dev
Are you sure you want to change the base?
Quest Teleop PR#1 #962
Conversation
Greptile SummaryImplements Quest3 VR teleoperation with dual-arm control through WebSocket/WebXR. The architecture follows a clean separation: Key Changes
Issues Found
Confidence Score: 4/5
Important Files Changed
Sequence DiagramsequenceDiagram
participant Quest3 as Quest3 Browser
participant FastAPI as FastAPI Server
participant Q3Module as Quest3TeleopModule
participant Controller as TeleopRobotController
participant Driver as Robot Driver
Quest3->>FastAPI: Open https://ip:8443 (WebXR)
FastAPI->>Quest3: Serve index.html
Quest3->>FastAPI: WebSocket handshake (role: teleop)
FastAPI->>Quest3: handshake_ack
Note over Quest3: User presses X button
Quest3->>FastAPI: {type: "start_teleop"}
FastAPI->>Q3Module: _handle_x_button("start_teleop")
Q3Module->>Q3Module: calibrate_vr() - capture initial poses
Q3Module->>FastAPI: {type: "teleop_started"}
FastAPI->>Quest3: Success response
loop Tracking Loop (60 FPS)
Quest3->>FastAPI: Controller tracking data
FastAPI->>Q3Module: _on_tracking_data(poses, grippers)
Q3Module->>Q3Module: update_controller_poses()
Q3Module->>Q3Module: _compute_and_publish_deltas()
Q3Module->>Controller: Publish delta poses (LCM)
Note over Controller: First delta triggers auto-calibration
Controller->>Driver: get_state() RPC
Driver->>Controller: Robot initial pose
Controller->>Controller: Apply delta: target = initial + delta
Controller->>Driver: Publish cartesian command
Driver->>Driver: Execute robot motion
end
Note over Quest3: User presses X again
Quest3->>FastAPI: {type: "stop_teleop"}
FastAPI->>Q3Module: _handle_x_button("stop_teleop")
Q3Module->>Q3Module: reset_calibration()
Q3Module->>FastAPI: {type: "teleop_stopped"}
FastAPI->>Quest3: Success response
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
16 files reviewed, 1 comment
| # close all connections | ||
| await self.manager.disconnect() | ||
|
|
||
| """Stop the FastAPI server asynchronously.""" | ||
| if self.server: | ||
| logger.info("Stopping FastAPI server...") | ||
| self.server.should_exit = True | ||
| await asyncio.sleep(0.1) # Give time for graceful shutdown |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
syntax: await self.manager.disconnect() on line 404 is called without arguments, but ConnectionManager.disconnect() method requires a websocket parameter
| # close all connections | |
| await self.manager.disconnect() | |
| """Stop the FastAPI server asynchronously.""" | |
| if self.server: | |
| logger.info("Stopping FastAPI server...") | |
| self.server.should_exit = True | |
| await asyncio.sleep(0.1) # Give time for graceful shutdown | |
| async def stop_async(self) -> None: | |
| """Stop the FastAPI server asynchronously.""" | |
| if self.server: | |
| logger.info("Stopping FastAPI server...") | |
| self.server.should_exit = True | |
| await asyncio.sleep(0.1) # Give time for graceful shutdown |
|
Feedback required:
Adding Implementation status notes here - #892
|
| config: BaseTeleopConfig | ||
|
|
||
| # LCM Output topics | ||
| left_controller_delta: Out[PoseStamped] = None # type: ignore[assignment] |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why not just output a transform and then the left and right PoseStamped objects, such that much of your operations / transformations on pose can be done via a type rather core implementation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Planning to do something similar in the next PR. Having head pos along with left and right PostStamped.
Although, if I have the transforms, I won't have the VR_world to robot_base transform. I need to depend on relative motion of the controllers; thus, had it compute this way to keep it generalizable.
If go2, we can infer the same output as the magnitudes of velocities and move the robot. Without any frame dependency (for now)
| # ========================================================================= | ||
|
|
||
| @rpc | ||
| def calibrate_vr(self) -> dict[str, Any]: |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This should definitely not be here. base teleop needs to work with any teleop, even an xbox controller or an iphone imu
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Sorry, its in the right place, typo with the naming, changing it.
| return {"success": True, "message": "Calibration reset - recalibrate to resume"} | ||
|
|
||
| @rpc | ||
| def is_vr_calibrated(self) -> bool: |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nothing mentioning VR should be in the generic teleop module
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Similar issue, renaming methods
| Dictionary with status information | ||
| """ | ||
| return { | ||
| "is_calibrated": self._is_calibrated, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This needs to be an Enum
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Done
| current_time = time.time() | ||
|
|
||
| # Left controller delta | ||
| if self.config.enable_left_arm and self._left_controller_initial is not None: |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Slop
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Updated!
4568283 to
6e2d4e4
Compare
|
Too many files changed for review. |
6e2d4e4 to
e300084
Compare
e300084 to
350721e
Compare
| logger.info(f"Captured controller {self._get_label(idx)} initial: {pose_obj}") | ||
| else: | ||
| return { | ||
| "success": False, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Needs type no json shit
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
removed json type returns
| control_loop_hz: float = 50.0 | ||
|
|
||
|
|
||
| class VRTeleopModule(BaseTeleopModule): |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Needs to be VRTeleopModule(BaseTeleopModule[VRTeleopModuleConfig]) or whatever the syntax is i forget
| super().__init__(*args, **kwargs) | ||
|
|
||
| self._lcm_lock = threading.Lock() | ||
| self._lcm_controller_poses: dict[int, NDArray[np.float64] | None] = { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Needs a type for ControllerPose cant just have a dict of random floats here bc need cleanoperations on this tyoe
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
and then you can do stuff like vector3 type conversions there
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Added ControllerPose msg type, shifted all math
| if self.vr_left_transform and self.vr_left_transform.transport: | ||
| self.vr_left_transform.subscribe( | ||
| lambda msg, idx=left_index: self._on_lcm_transform(idx, msg) # type: ignore[misc] | ||
| ) | ||
|
|
||
| if self.vr_right_transform and self.vr_right_transform.transport: | ||
| self.vr_right_transform.subscribe( | ||
| lambda msg, idx=right_index: self._on_lcm_transform(idx, msg) # type: ignore[misc] | ||
| ) | ||
|
|
||
| if self.vr_trigger_0 and self.vr_trigger_0.transport: | ||
| self.vr_trigger_0.subscribe( | ||
| lambda msg, idx=left_index: self._on_lcm_trigger(idx, msg) # type: ignore[misc] | ||
| ) | ||
|
|
||
| if self.vr_trigger_1 and self.vr_trigger_1.transport: | ||
| self.vr_trigger_1.subscribe( | ||
| lambda msg, idx=right_index: self._on_lcm_trigger(idx, msg) # type: ignore[misc] | ||
| ) | ||
|
|
||
| if self.teleop_enable and self.teleop_enable.transport: | ||
| self.teleop_enable.subscribe(self._on_lcm_teleop_enable) | ||
|
|
||
| logger.info("VR Teleoperation Module started") |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Clean up can use match here
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Not exactly pattern matching over a value, but can use loop
dimos/utils/teleop_transforms.py
Outdated
| ) | ||
|
|
||
|
|
||
| def _to_twist( |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This cannot be here needs to be in the type for the thing i said earlier we need which is ControllerPoseType which will wrap Vector3 and handle all conversion to vector3 very cleanly
dimos/msgs/geometry_msgs/Pose.py
Outdated
|
|
||
| return Pose(new_position, new_orientation) | ||
|
|
||
| def __sub__(self, other: Pose | PoseConvertable) -> Pose: |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@leshy I assume there are issues with this due to other Pose operations. add for example has some checks
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This might be ok if it passes tests.. but this PoseConvertible is a bit goofy code I wrote very very early, probably we should be strict and only add and sub between Pose and not try and do fancy auto type conversions.
idk if he can easily remove PoseConvertable and pass tests from both add and sub if yes, great. if not, make sub only support Pose so that my wrong approach doesn't spread
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Made sub only support Pose and it passes tests.
| controller_delta_0: Out[PoseStamped] = None # type: ignore | ||
| trigger_value_0: Out[Bool] = None # type: ignore | ||
| controller_delta_1: Out[PoseStamped] = None # type: ignore | ||
| trigger_value_1: Out[Bool] = None # type: ignore | ||
| controller_delta_2: Out[Twist] = None # type: ignore | ||
| trigger_value_2: Out[Bool] = None # type: ignore | ||
| controller_delta_3: Out[Twist] = None # type: ignore | ||
| trigger_value_3: Out[Bool] = None # type: ignore |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
What if we are teleoping without controllers?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
no controllers mean no subscribers to read data from vr. Checks to make sure at least one output_type is specified
dimos/teleop/teleop_blueprints.py
Outdated
| ("vr_left_transform", LCMTransform): LCMTransport("/vr_left_transform", LCMTransform), | ||
| ("vr_right_transform", LCMTransform): LCMTransport("/vr_right_transform", LCMTransform), | ||
| ("vr_trigger_0", Float32): LCMTransport("/vr_trigger_0", Float32), | ||
| ("vr_trigger_1", Float32): LCMTransport("/vr_trigger_1", Float32), |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
These are the defaults, so you can delete these lines.
Also, if you rename teleop_enable to vr_teleop_enable in the Module, you don't need to change the topic here.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Actually yeah. renamed and deleted mentions
| vr_left_transform: In[LCMTransform] = None # type: ignore[assignment] | ||
| vr_right_transform: In[LCMTransform] = None # type: ignore[assignment] | ||
| vr_trigger_0: In[Float32] = None # type: ignore[assignment] | ||
| vr_trigger_1: In[Float32] = None # type: ignore[assignment] | ||
| teleop_enable: In[Bool] = None # type: ignore[assignment] # X button calibration toggle |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
| vr_left_transform: In[LCMTransform] = None # type: ignore[assignment] | |
| vr_right_transform: In[LCMTransform] = None # type: ignore[assignment] | |
| vr_trigger_0: In[Float32] = None # type: ignore[assignment] | |
| vr_trigger_1: In[Float32] = None # type: ignore[assignment] | |
| teleop_enable: In[Bool] = None # type: ignore[assignment] # X button calibration toggle | |
| vr_left_transform: In[LCMTransform] | |
| vr_right_transform: In[LCMTransform] | |
| vr_trigger_0: In[Float32] | |
| vr_trigger_1: In[Float32] | |
| teleop_enable: In[Bool] # X button calibration toggle |
= None isn't needed anymore
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Got it, updating all declarations!
| controller_delta_0: Out[PoseStamped] = None # type: ignore | ||
| trigger_value_0: Out[Bool] = None # type: ignore | ||
| controller_delta_1: Out[PoseStamped] = None # type: ignore | ||
| trigger_value_1: Out[Bool] = None # type: ignore | ||
| controller_delta_2: Out[Twist] = None # type: ignore | ||
| trigger_value_2: Out[Bool] = None # type: ignore | ||
| controller_delta_3: Out[Twist] = None # type: ignore | ||
| trigger_value_3: Out[Bool] = None # type: ignore |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
| controller_delta_0: Out[PoseStamped] = None # type: ignore | |
| trigger_value_0: Out[Bool] = None # type: ignore | |
| controller_delta_1: Out[PoseStamped] = None # type: ignore | |
| trigger_value_1: Out[Bool] = None # type: ignore | |
| controller_delta_2: Out[Twist] = None # type: ignore | |
| trigger_value_2: Out[Bool] = None # type: ignore | |
| controller_delta_3: Out[Twist] = None # type: ignore | |
| trigger_value_3: Out[Bool] = None # type: ignore | |
| controller_delta_0: Out[PoseStamped] | |
| trigger_value_0: Out[Bool] | |
| controller_delta_1: Out[PoseStamped] | |
| trigger_value_1: Out[Bool] | |
| controller_delta_2: Out[Twist] | |
| trigger_value_2: Out[Bool] | |
| controller_delta_3: Out[Twist] | |
| trigger_value_3: Out[Bool] |
dimos/utils/teleop_transforms.py
Outdated
| def compute_active_indices(output_types: list[type]) -> list[int]: | ||
| """Compute active indices from output types. | ||
| Example: | ||
| [PoseStamped, Twist] → [0, 2] | ||
| [Twist, PoseStamped] → [2, 0] | ||
| [PoseStamped, PoseStamped] → [0, 1] | ||
| [Twist, Twist] → [2, 3] | ||
| """ | ||
| indices: list[int] = [] | ||
| used_indices: set[int] = set() | ||
|
|
||
| for output_type in output_types: | ||
| available = OUTPUT_TYPE_INDICES.get(output_type, []) | ||
| for idx in available: | ||
| if idx not in used_indices: | ||
| indices.append(idx) | ||
| used_indices.add(idx) | ||
| break | ||
| else: | ||
| raise ValueError(f"No available index for output type {output_type.__name__}") | ||
|
|
||
| return indices |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This docstring is quite puzzling.
My understanding now is that it maps to the first available output in controller_delta_0, ..., _3 in BaseTeleopModule. Is this correct?
If so, it should be a method on that class as it's specific to it.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yes, as we can't declare them at run time - initiating all potential streams we need, which are mapped later using this method - it can be 2xPoseStamped, 2xTwist, or one each.
mm... Implementation didn't feel too traditional to put it inside the class.
But, yes, this is super specific to class, I will move it
| ] | ||
| for stream, handler in subscriptions: | ||
| if stream and stream.transport: | ||
| stream.subscribe(handler) # type: ignore[misc, arg-type] |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
| stream.subscribe(handler) # type: ignore[misc, arg-type] | |
| self._disposables.add(Disposable(stream.subscribe(handler))) # type: ignore[misc, arg-type] |
You also have to import Disposable
| """ | ||
|
|
||
| @classmethod | ||
| def from_pose(cls, pose: Pose) -> ControllerPose: |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Personally, I'm not a fan of this.
If you need ways to convert Pose to PoseStamped or Twist, you can just write standalone functions for that. There's no need to introduce a new type but with no new fields.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Agreed, also shouldn't use Pose at all but generate PoseStamped straight from your hardware, we always want the pose with the frame_id and timestamp.
I also don't understand exactly why we are converting pose to twist? We should probably be generating twist in some intelligent way when holding a button or something, otherwise scratching your butt would make a robot go insane?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Okay, I can get timestamp and frame_id from the controller.
Reg Twist: I have start/stop (X button), so hoping this should be enough to control it well. Planning further to have spherical volume of say 5cm radius, around caliberated point - which would not send any twist pos (zeros), and moving controller further would start velocities.. something safe.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
And, reg ControllerPose type...
As an alternative, I can have PoseStamped in place of ControllerPoseStamped (Now that we need to carry timestamps... It shall be ControllerPoseStamped) and use the 'to_twist' methods in utils. What do you suggest? @spomichter
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Sorted!
Replaced ControllerPose with Pose, Added standalone functions in utils/teleop_transform.py.
frame_id and timestamps, added as received from hardware.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
so I still see ControllerPose file, this file shouldn't exist, geometry_msgs matches ros msgs, and ControllerPose isn't one
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I did remove the file. I don't find it anywhere. Can you lmk where you see it
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
aha it's gone, was some github stale diff issue
|
|
||
| return Twist(linear=linear, angular=angular) | ||
|
|
||
| def __sub__(self, other: Pose | PoseConvertable) -> ControllerPose: # type: ignore[override] |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'm not sure why you need this, but generally # type: ignore[override] is bad because it means you're not respecting the contract.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
without -> ControllerPose the method returns a Pose. ignore is added as this overrides parents __sub__ with different return type.
Anyways, I have removed entire ControllerPose msg type now
dimos/msgs/std_msgs/Float32.py
Outdated
| from dimos_lcm.std_msgs import Float32 as LCMFloat32 | ||
|
|
||
| try: | ||
| from std_msgs.msg import Float32 as ROSFloat32 # type: ignore[attr-defined] |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
why do we have ros stuff in this? don't just delete, pls explain, I want to understand why I keep seeing this in PRs
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
As mentioned below, took reference from other message type classes. If we don't need this, should we remove it from all those msg types?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
yes no ros needed
dimos/msgs/std_msgs/Float32.py
Outdated
| self.data = data | ||
|
|
||
| @classmethod | ||
| def from_ros_msg(cls, ros_msg: ROSFloat32) -> "Float32": |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
why did you write this conversion?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@classmethod
def from_ros_msg(cls, ros_msg: ROSBool) -> "Bool":
"""Create a Bool from a ROS std_msgs/Bool message.
@classmethod
def from_ros_msg(cls, ros_msg: ROSInt8) -> "Int8":
"""Create a Bool from a ROS std_msgs/Bool message.
Don't we want it? Int8, Bool types from std_msgs have this method. I assumed its a convention we follow
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I understand why it seems so :) this is part of an older ros integration we are obsoleting
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
this file seems teleop specific? pls put it in your teleop dir
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yes, they are. Maybe I can add them at dimos/teleop/utils/
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
also seems like something that belongs in teleop dir
[Check latest comment for updated info]
Quest3 Teleop Module Implementation
Features Included
BaseTeleopModule, publishes: (Max - 90Hz)PoseStamped(left and right controller poses)Float32(gripper trigger values)Quest3TeleopModulePose+ 2xBoolas required by robot modulesTopics viz