fixes for processors used in phone teleop

This commit is contained in:
Pepijn
2025-09-12 11:57:48 +02:00
parent f51272362c
commit 8e0f5cd052
4 changed files with 33 additions and 29 deletions
+4
View File
@@ -173,3 +173,7 @@ outputs/
# Dev folders
.cache/*
*.stl
*.urdf
*.xml
*.part
@@ -193,13 +193,13 @@ class EEBoundsAndSafety(RobotActionProcessorStep):
_last_pos: np.ndarray | None = field(default=None, init=False, repr=False)
_last_twist: np.ndarray | None = field(default=None, init=False, repr=False)
def action(self, act: RobotAction) -> RobotAction:
x = act.get("ee.x", None)
y = act.get("ee.y", None)
z = act.get("ee.z", None)
wx = act.get("ee.wx", None)
wy = act.get("ee.wy", None)
wz = act.get("ee.wz", None)
def action(self, action: RobotAction) -> RobotAction:
x = action.get("ee.x", None)
y = action.get("ee.y", None)
z = action.get("ee.z", None)
wx = action.get("ee.wx", None)
wy = action.get("ee.wy", None)
wz = action.get("ee.wz", None)
if None in (x, y, z, wx, wy, wz):
raise ValueError(
@@ -223,13 +223,13 @@ class EEBoundsAndSafety(RobotActionProcessorStep):
self._last_pos = pos
self._last_twist = twist
act["ee.x"] = float(pos[0])
act["ee.y"] = float(pos[1])
act["ee.z"] = float(pos[2])
act["ee.wx"] = float(twist[0])
act["ee.wy"] = float(twist[1])
act["ee.wz"] = float(twist[2])
return act
action["ee.x"] = float(pos[0])
action["ee.y"] = float(pos[1])
action["ee.z"] = float(pos[2])
action["ee.wx"] = float(twist[0])
action["ee.wy"] = float(twist[1])
action["ee.wz"] = float(twist[2])
return action
def reset(self):
"""Resets the last known position and orientation."""
+1 -1
View File
@@ -15,4 +15,4 @@
# limitations under the License.
from .config_phone import PhoneConfig
from .phone import Phone
from .teleop_phone import Phone
@@ -41,7 +41,7 @@ class MapPhoneActionToRobotAction(RobotActionProcessorStep):
platform: PhoneOS
_enabled_prev: bool = field(default=False, init=False, repr=False)
def action(self, act: RobotAction) -> RobotAction:
def action(self, action: RobotAction) -> RobotAction:
"""
Processes the phone action dictionary to create a robot action dictionary.
@@ -55,10 +55,10 @@ class MapPhoneActionToRobotAction(RobotActionProcessorStep):
ValueError: If 'pos' or 'rot' keys are missing from the input action.
"""
# Pop them from the action
enabled = bool(act.pop("phone.enabled", 0))
pos = act.pop("phone.pos", None)
rot = act.pop("phone.rot", None)
inputs = act.pop("phone.raw_inputs", {})
enabled = bool(action.pop("phone.enabled", 0))
pos = action.pop("phone.pos", None)
rot = action.pop("phone.rot", None)
inputs = action.pop("phone.raw_inputs", {})
if pos is None or rot is None:
raise ValueError("pos and rot must be present in action")
@@ -76,15 +76,15 @@ class MapPhoneActionToRobotAction(RobotActionProcessorStep):
) # Positive if a is pressed, negative if b is pressed, 0 if both or neither are pressed
# For some actions we need to invert the axis
act["enabled"] = enabled
act["target_x"] = -pos[1] if enabled else 0.0
act["target_y"] = pos[0] if enabled else 0.0
act["target_z"] = pos[2] if enabled else 0.0
act["target_wx"] = rotvec[1] if enabled else 0.0
act["target_wy"] = rotvec[0] if enabled else 0.0
act["target_wz"] = -rotvec[2] if enabled else 0.0
act["gripper"] = gripper # Still send gripper action when disabled
return act
action["enabled"] = enabled
action["target_x"] = -pos[1] if enabled else 0.0
action["target_y"] = pos[0] if enabled else 0.0
action["target_z"] = pos[2] if enabled else 0.0
action["target_wx"] = rotvec[1] if enabled else 0.0
action["target_wy"] = rotvec[0] if enabled else 0.0
action["target_wz"] = -rotvec[2] if enabled else 0.0
action["gripper"] = gripper # Still send gripper action when disabled
return action
def transform_features(
self, features: dict[PipelineFeatureType, dict[str, PolicyFeature]]