Merge pull request #119 from utn-mi/walter/fix-vive-pose

fix: pose calculation and control for vive teleoperation

Author: juelg

python/rcsss/__init__.py

@@ -3,11 +3,11 @@
 import pathlib
 import site
 
-from rcsss import camera, desk, sim
+from rcsss import camera, control, sim
 from rcsss._core import __version__, common, hw
 
 scenes = {
     path.stem: path / "scene.mjb" for path in (pathlib.Path(site.getsitepackages()[0]) / "rcsss" / "scenes").glob("*")
 }
 
-__all__ = ["__doc__", "__version__", "common", "hw", "sim", "desk", "camera", "scenes"]
+__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control"]

python/rcsss/cli/main.py

@@ -6,6 +6,7 @@
 
 import pyrealsense2 as rs
 import rcsss
+import rcsss.control.fr3_desk
 import typer
 from PIL import Image
 from rcsss.camera.realsense import RealSenseCameraSet
@@ -113,7 +114,7 @@ def home(
 ):
     """Moves the FR3 to home position"""
     cfg = read_config_yaml(path)
-    rcsss.desk.home(ip, cfg.hw.username, cfg.hw.password, shut)
+    rcsss.control.fr3_desk.home(ip, cfg.hw.username, cfg.hw.password, shut)
 
 
 @fr3_app.command()
@@ -123,7 +124,7 @@ def lock(
 ):
     """Locks the robot."""
     cfg = read_config_yaml(path)
-    rcsss.desk.lock(ip, cfg.hw.username, cfg.hw.password)
+    rcsss.control.fr3_desk.lock(ip, cfg.hw.username, cfg.hw.password)
 
 
 @fr3_app.command()
@@ -133,8 +134,8 @@ def unlock(
 ):
     """Prepares the robot by unlocking the joints and putting the robot into the FCI mode."""
     cfg = read_config_yaml(path)
-    rcsss.desk.unlock(ip, cfg.hw.username, cfg.hw.password)
-    with rcsss.desk.Desk(ip, cfg.hw.username, cfg.hw.password) as d:
+    rcsss.control.fr3_desk.unlock(ip, cfg.hw.username, cfg.hw.password)
+    with rcsss.control.fr3_desk.Desk(ip, cfg.hw.username, cfg.hw.password) as d:
         d.activate_fci()
 
 
@@ -148,12 +149,14 @@ def fci(
     """Puts the robot into FCI mode, optionally unlocks the robot. Waits for ctrl+c to exit."""
     cfg = read_config_yaml(path)
     try:
-        with rcsss.desk.FCI(rcsss.desk.Desk(ip, cfg.hw.username, cfg.hw.password), unlock=unlock):
+        with rcsss.control.fr3_desk.FCI(
+            rcsss.control.fr3_desk.Desk(ip, cfg.hw.username, cfg.hw.password), unlock=unlock
+        ):
             while True:
                 sleep(1)
     except KeyboardInterrupt:
         if shutdown:
-            rcsss.desk.shutdown(ip, cfg.hw.username, cfg.hw.password)
+            rcsss.control.fr3_desk.shutdown(ip, cfg.hw.username, cfg.hw.password)
 
 
 @fr3_app.command()
@@ -164,7 +167,7 @@ def guiding_mode(
 ):
     """Enables or disables guiding mode."""
     cfg = read_config_yaml(path)
-    rcsss.desk.guiding_mode(ip, cfg.hw.username, cfg.hw.password, disable)
+    rcsss.control.fr3_desk.guiding_mode(ip, cfg.hw.username, cfg.hw.password, disable)
 
 
 @fr3_app.command()
@@ -174,7 +177,7 @@ def shutdown(
 ):
     """Shuts the robot down"""
     cfg = read_config_yaml(path)
-    rcsss.desk.shutdown(ip, cfg.hw.username, cfg.hw.password)
+    rcsss.control.fr3_desk.shutdown(ip, cfg.hw.username, cfg.hw.password)
 
 
 @fr3_app.command()
@@ -194,7 +197,9 @@ def record(
         with ExitStack() as stack:
             for r_ip in name2ip.values():
                 stack.enter_context(
-                    rcsss.desk.Desk.fci(r_ip, username=cfg.hw.username, password=cfg.hw.password, unlock=True)
+                    rcsss.control.fr3_desk.Desk.fci(
+                        r_ip, username=cfg.hw.username, password=cfg.hw.password, unlock=True
+                    )
                 )
 
             p = PoseList.load(name2ip, lpaths, cfg.hw.urdf_model_path)
@@ -203,7 +208,9 @@ def record(
     else:
         with ExitStack() as stack:
             gms = [
-                rcsss.desk.Desk.guiding_mode(r_ip, username=cfg.hw.username, password=cfg.hw.password, unlock=True)
+                rcsss.control.fr3_desk.Desk.guiding_mode(
+                    r_ip, username=cfg.hw.username, password=cfg.hw.password, unlock=True
+                )
                 for r_ip in name2ip.values()
             ]
             for gm in gms:

python/rcsss/control/liveplot.py

@@ -11,8 +11,8 @@
 port = 54322
 FMT = "!" + 7 * "d" + "i"
 
-buffsize = 30
-# buffsize = 300
+# buffsize = 30
+buffsize = 300
 # Order of buffers: x, y, z, rx, ry, rz
 
 titles = [["x", "rx"], ["y", "ry"], ["z", "rz"]]
@@ -64,4 +64,4 @@
 
         plt.draw()
         plt.pause(0.001)
-        plt.pause(0.5)
+        # plt.pause(0.5)

python/rcsss/control/vive.py

@@ -2,7 +2,7 @@
 import threading
 from enum import IntFlag, auto
 from socket import AF_INET, SOCK_DGRAM, socket
-from struct import unpack  # , pack
+from struct import pack, unpack
 
 import gymnasium as gym
 import numpy as np
@@ -34,8 +34,8 @@
 EGO_LOCK = False
 VIVE_HOST = "192.168.100.1"
 VIVE_PORT = 54321
-USE_REAL_ROBOT = False
-INCLUDE_ROTATION = False
+USE_REAL_ROBOT = True
+INCLUDE_ROTATION = True
 ROBOT_IP = "192.168.101.1"
 
 
@@ -77,7 +77,7 @@ def __init__(
     def next_action(self) -> Pose:
         transform = Pose(
             translation=self._last_controller_pose.translation() - self._offset_pose.translation(),
-            quaternion=(self._offset_pose.inverse() * self._last_controller_pose).rotation_q(),
+            quaternion=(self._last_controller_pose * self._offset_pose.inverse()).rotation_q(),
         )
         return (
             self._ego_transform
@@ -93,11 +93,11 @@ def get_last_controller_pose(self) -> Pose:
     def run(self):
         warning_raised = False
 
-        with socket(AF_INET, SOCK_DGRAM) as sock:
-            # with socket(AF_INET, SOCK_DGRAM) as send_sock:
+        with socket(AF_INET, SOCK_DGRAM) as sock, socket(AF_INET, SOCK_DGRAM) as send_sock:
             sock.settimeout(2)
+            sock.setblocking(False)
             sock.bind((self._host, self._port))
-            # send_sock.connect(("127.0.0.1", self._port + 1))
+            send_sock.connect(("127.0.0.1", self._port + 1))
             while not self._exit_requested:
                 try:
                     unpacked = unpack(UDPViveActionServer.FMT, sock.recv(7 * 8 + 4))
@@ -155,18 +155,18 @@ def run(self):
                         self._last_controller_pose = last_controller_pose
 
                         # plot current offset with liveplot.py
-                        # transform = Pose(
-                        #     translation=self._last_controller_pose.translation() - self._offset_pose.translation(),
-                        #     quaternion=(self._offset_pose.inverse() * self._last_controller_pose).rotation_q(),
-                        # )
-                        # offset = (
-                        #     self._ego_transform
-                        #     * UDPViveActionServer.transform_from_openxr
-                        #     * transform
-                        #     * UDPViveActionServer.transform_from_openxr.inverse()
-                        #     * self._ego_transform.inverse()
-                        # )
-                        # send_sock.sendall(pack(UDPViveActionServer.FMT, *offset.rotation_q(), *offset.translation(), 0))
+                        transform = Pose(
+                            translation=self._last_controller_pose.translation() - self._offset_pose.translation(),
+                            quaternion=(self._last_controller_pose * self._offset_pose.inverse()).rotation_q(),
+                        )
+                        offset = (
+                            self._ego_transform
+                            * UDPViveActionServer.transform_from_openxr
+                            * transform
+                            * UDPViveActionServer.transform_from_openxr.inverse()
+                            * self._ego_transform.inverse()
+                        )
+                        send_sock.sendall(pack(UDPViveActionServer.FMT, *offset.rotation_q(), *offset.translation(), 0))
 
                     if Button(int(unpacked[7])) & self._grp_btn and not Button(int(self._buttons)) & self._grp_btn:
                         # just pressed
@@ -210,7 +210,7 @@ def hw():
         rcfg = rcsss.hw.FR3Config()
         rcfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset())
         rcfg.speed_factor = 0.2
-        # rcfg.controller = rcsss.hw.IKController.robotics_library
+        rcfg.controller = rcsss.hw.IKController.robotics_library
         robot.set_parameters(rcfg)
 
         # env = FR3Env(robot, ControlMode.CARTESIAN_TQuart)

python/rcsss/envs/base.py

@@ -213,18 +213,21 @@ def step(self, action: CartOrJointContType) -> tuple[ArmObsType, float, bool, bo
             raise RuntimeError(msg)
 
         if self.get_base_control_mode() == ControlMode.JOINTS and (
-            self.prev_action is None or not np.allclose(action_dict[self.joints_key], self.prev_action[self.joints_key])
+            self.prev_action is None
+            or not np.allclose(action_dict[self.joints_key], self.prev_action[self.joints_key], atol=1e-03, rtol=0)
         ):
             # cast is needed because typed dicts cannot be checked at runtime
             self.robot.set_joint_position(action_dict[self.joints_key])
         elif self.get_base_control_mode() == ControlMode.CARTESIAN_TRPY and (
-            self.prev_action is None or not np.allclose(action_dict[self.trpy_key], self.prev_action[self.trpy_key])
+            self.prev_action is None
+            or not np.allclose(action_dict[self.trpy_key], self.prev_action[self.trpy_key], atol=1e-03, rtol=0)
         ):
             self.robot.set_cartesian_position(
                 common.Pose(translation=action_dict[self.trpy_key][:3], rpy_vector=action_dict[self.trpy_key][3:])
             )
         elif self.get_base_control_mode() == ControlMode.CARTESIAN_TQuart and (
-            self.prev_action is None or not np.allclose(action_dict[self.tquart_key], self.prev_action[self.tquart_key])
+            self.prev_action is None
+            or not np.allclose(action_dict[self.tquart_key], self.prev_action[self.tquart_key], atol=1e-03, rtol=0)
         ):
             self.robot.set_cartesian_position(
                 common.Pose(translation=action_dict[self.tquart_key][:3], quaternion=action_dict[self.tquart_key][3:])

src/hw/FR3.cpp

@@ -39,11 +39,13 @@ FR3::FR3(const std::string &ip, const std::optional<std::string> &filename,
   if (filename.has_value()) {
     this->rl = RL();
 
-    this->rl.value().mdl = rl::mdl::UrdfFactory().create(filename.value());
-    this->rl.value().kin =
-        std::dynamic_pointer_cast<rl::mdl::Kinematic>(this->rl.value().mdl);
-    this->rl.value().ik = std::make_shared<rl::mdl::JacobianInverseKinematics>(
-        this->rl.value().kin.get());
+    this->rl->mdl = rl::mdl::UrdfFactory().create(filename.value());
+    this->rl->kin =
+        std::dynamic_pointer_cast<rl::mdl::Kinematic>(this->rl->mdl);
+    this->rl->ik = std::make_shared<rl::mdl::JacobianInverseKinematics>(
+        this->rl->kin.get());
+    this->rl->ik->setRandomRestarts(0);
+    this->rl->ik->setEpsilon(1e-3);
   }
 }
 
@@ -218,17 +220,17 @@ void FR3::set_cartesian_position_rl(const common::Pose &pose) {
     throw rl::mdl::Exception(
         "No file for robot model was provided. Cannot use RL for IK.");
   }
-  this->rl.value().kin->setPosition(this->get_joint_position());
-  this->rl.value().kin->forwardPosition();
+  this->rl->kin->setPosition(this->get_joint_position());
+  this->rl->kin->forwardPosition();
   rcs::common::Pose new_pose = pose * this->cfg.tcp_offset.inverse();
 
-  this->rl.value().ik->addGoal(new_pose.affine_matrix(), 0);
-  bool success = this->rl.value().ik->solve();
+  this->rl->ik->addGoal(new_pose.affine_matrix(), 0);
+  bool success = this->rl->ik->solve();
   if (success) {
     // is this forward needed and is it mabye possible to call
     // this on the model?
-    this->rl.value().kin->forwardPosition();
-    this->set_joint_position(this->rl.value().kin->getPosition());
+    this->rl->kin->forwardPosition();
+    this->set_joint_position(this->rl->kin->getPosition());
   } else {
     // throw error
     throw rl::mdl::Exception("IK failed");

src/sim/FR3.cpp

@@ -64,6 +64,8 @@ void FR3::construct() {
       std::make_shared<rl::mdl::JacobianInverseKinematics>(this->rl.kin.get());
   this->rl.ik->setDuration(
       std::chrono::milliseconds(this->cfg.ik_duration_in_milliseconds));
+  this->rl.ik->setRandomRestarts(0);
+  this->rl.ik->setEpsilon(1e-3);
   this->init_ids();
   this->sim->register_cb(std::bind(&FR3::collision_callback, this),
                          this->cfg.seconds_between_callbacks);