diff --git a/deoxys/deoxys/scripts/print_robot_state.py b/deoxys/deoxys/scripts/print_robot_state.py new file mode 100644 index 0000000..e4dd3d9 --- /dev/null +++ b/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() diff --git a/deoxys/deoxys/scripts/reset_robot_joints.py b/deoxys/deoxys/scripts/reset_robot_joints.py new file mode 100644 index 0000000..10a5e04 --- /dev/null +++ b/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() diff --git a/deoxys/setup.py b/deoxys/setup.py index 07d5159..39746f7 100644 --- a/deoxys/setup.py +++ b/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', ] }, )