Add missing files for deoxys scripts

deoxys/deoxys/scripts/print_robot_state.py

@@ -0,0 +1,63 @@
+"""Moving robot joint positions to initial pose for starting new experiments."""
+import argparse
+import pickle
+import threading
+import time
+from pathlib import Path
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+from deoxys import config_root
+from deoxys.franka_interface import FrankaInterface
+from deoxys.utils import YamlConfig
+from deoxys.utils.input_utils import input2action
+from deoxys.utils.io_devices import SpaceMouse
+from deoxys.utils.log_utils import get_deoxys_example_logger
+
+import pprint
+
+logger = get_deoxys_example_logger()
+
+
+def parse_args():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--interface-cfg", type=str, default="charmander.yml")
+    parser.add_argument(
+        "--controller-cfg", type=str, default="joint-position-controller.yml"
+    )
+    parser.add_argument(
+        "--folder", type=Path, default="data_collection_example/example_data"
+    )
+
+    args = parser.parse_args()
+    return args
+
+
+def main():
+    args = parse_args()
+
+    robot_interface = FrankaInterface(
+        config_root + f"/{args.interface_cfg}", use_visualizer=False
+    )
+    controller_cfg = YamlConfig(config_root + f"/{args.controller_cfg}").as_easydict()
+
+    controller_type = "JOINT_POSITION"
+
+    pp = pprint.PrettyPrinter(indent=2)
+    while True:
+        if len(robot_interface._state_buffer) > 0:
+            logger.info(f"Current Robot joint: {np.round(robot_interface.last_q, 3)}")
+            last_eef_rot, last_eef_pos = robot_interface.last_eef_rot_and_pos
+            logger.info(f"Current eef rotation: ")
+            pp.pprint([list(i) for i in last_eef_rot])
+
+            logger.info(f"Current eef position: ")
+            pp.pprint([list(i)[0] for i in last_eef_pos])
+
+            break
+    robot_interface.close()
+
+
+if __name__ == "__main__":
+    main()

deoxys/deoxys/scripts/reset_robot_joints.py

@@ -0,0 +1,88 @@
+"""Moving robot joint positions to initial pose for starting new experiments."""
+import argparse
+import pickle
+import threading
+import time
+from pathlib import Path
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+from deoxys import config_root
+from deoxys.franka_interface import FrankaInterface
+from deoxys.utils import YamlConfig
+from deoxys.utils.input_utils import input2action
+from deoxys.utils.io_devices import SpaceMouse
+from deoxys.utils.log_utils import get_deoxys_example_logger
+
+logger = get_deoxys_example_logger()
+
+
+def parse_args():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--interface-cfg", type=str, default="charmander.yml")
+    parser.add_argument(
+        "--controller-cfg", type=str, default="joint-position-controller.yml"
+    )
+    parser.add_argument(
+        "--folder", type=Path, default="data_collection_example/example_data"
+    )
+
+    args = parser.parse_args()
+    return args
+
+
+def main():
+    args = parse_args()
+
+    robot_interface = FrankaInterface(
+        config_root + f"/{args.interface_cfg}", use_visualizer=False
+    )
+    controller_cfg = YamlConfig(config_root + f"/{args.controller_cfg}").as_easydict()
+
+    controller_type = "JOINT_POSITION"
+
+    # Golden resetting joints
+    reset_joint_positions = [
+        0.09162008114028396,
+        -0.19826458111314524,
+        -0.01990020486871322,
+        -2.4732269941140346,
+        -0.01307073642274261,
+        2.30396583422025,
+        0.8480939705504309,
+    ]
+
+    # This is for varying initialization of joints a little bit to
+    # increase data variation.
+    reset_joint_positions = [
+        e + np.clip(np.random.randn() * 0.005, -0.005, 0.005)
+        for e in reset_joint_positions
+    ]
+    action = reset_joint_positions + [-1.0]
+
+    while True:
+        if len(robot_interface._state_buffer) > 0:
+            logger.info(f"Current Robot joint: {np.round(robot_interface.last_q, 3)}")
+            logger.info(f"Desired Robot joint: {np.round(robot_interface.last_q_d, 3)}")
+
+            if (
+                np.max(
+                    np.abs(
+                        np.array(robot_interface._state_buffer[-1].q)
+                        - np.array(reset_joint_positions)
+                    )
+                )
+                < 1e-3
+            ):
+                break
+        robot_interface.control(
+            controller_type=controller_type,
+            action=action,
+            controller_cfg=controller_cfg,
+        )
+    robot_interface.close()
+
+
+if __name__ == "__main__":
+    main()

deoxys/setup.py

@@ -30,7 +30,8 @@
                 'deoxys.get_controller_list=deoxys.scripts.get_deoxys_info:default_controller_list',
                 'deoxys.get_controller_info=deoxys.scripts.get_deoxys_info:default_all_controller_info',
                 'deoxys.reset_joints=deoxys.scripts.reset_robot_joints:main',
-                'deoxys.velocity_example=examples.cartesian_velocity_control:main'
+                'deoxys.print_robot_state=deoxys.scripts.print_robot_state:main',
+                'deoxys.velocity_example=examples.cartesian_velocity_control:main',
         ]
     },
 )