diff --git a/gello/agents/gello_agent.py b/gello/agents/gello_agent.py index 0cc7e42..b332266 100644 --- a/gello/agents/gello_agent.py +++ b/gello/agents/gello_agent.py @@ -50,17 +50,19 @@ PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = { "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT94VP8U-if00-port0": DynamixelRobotConfig( joint_ids=(1, 2, 3, 4, 5, 6, 7), + # 7*np.pi/4, 4*np.pi/4, 2*np.pi/4, 0*np.pi/4, 4*np.pi/4, 4*np.pi/4, 10*np.pi/4 + # 3*np.pi/4, 4*np.pi/4, 14*np.pi/4, 0*np.pi/4, -4*np.pi/4, 4*np.pi/4, 2*np.pi/4 joint_offsets=( - 1 * np.pi / 2, - 3 * np.pi / 2, - 2 * np.pi / 2, - 1 * np.pi / 2, - 1 * np.pi / 2, - 1 * np.pi / 2, - 2 * np.pi / 2, + 3 * np.pi / 4, + 4 * np.pi / 4, + 14 * np.pi / 4, + 0 * np.pi / 4, + -4 * np.pi / 4, + 4 * np.pi / 4, + 2 * np.pi / 4, ), joint_signs=(1, -1, 1, 1, 1, 1, 1), - gripper_config=(8, 115.024609375, 73.224609375), + gripper_config=(8, 330.79609375, 288.99609375), ), # panda # "/dev/cu.usbserial-FT3M9NVB": DynamixelRobotConfig( diff --git a/scripts/gello_get_offset.py b/scripts/gello_get_offset.py index 4e5aa8c..aca790a 100644 --- a/scripts/gello_get_offset.py +++ b/scripts/gello_get_offset.py @@ -65,7 +65,7 @@ def get_config(args: Args) -> None: best_offset = 0 best_error = 1e6 for offset in np.linspace( - -8 * np.pi, 8 * np.pi, 8 * 4 + 1 + -8 * np.pi, 8 * np.pi, 8 * 8 + 1 ): # intervals of pi/2 error = get_error(offset, i, curr_joints) if error < best_error: @@ -76,7 +76,7 @@ def get_config(args: Args) -> None: print("best offsets : ", [f"{x:.3f}" for x in best_offsets]) print( "best offsets function of pi: [" - + ", ".join([f"{int(np.round(x/(np.pi/2)))}*np.pi/2" for x in best_offsets]) + + ", ".join([f"{int(np.round(x/(np.pi/4)))}*np.pi/4" for x in best_offsets]) + " ]", ) if args.gripper: