robossembler/gello_software
7
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Merge 69f2453e2d into daae81f4a7

Browse source
This commit is contained in:
yuwei-will-xia 2024-12-18 22:07:43 +00:00 • committed by GitHub
commit 8733ce03ea
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
8 changed files with 224 additions and 96 deletions
Download patch file Download diff file Expand all files Collapse all files

17
.bkpBUILD_bak.bazel Normal file
View file

@ -0,0 +1,17 @@
load("@rules_python//python:defs.bzl", "py_library", "py_binary")
load("@python_deps//:requirements.bzl", "requirement")
py_library(
name = "gello_software_repo",
srcs = glob(["*.py", "**/*.py"]),
deps = [
requirement("numpy"),
requirement("torchvision"),
requirement("torch"),
requirement("ultralytics"),
requirement("opencv-python"),
requirement("pyquaternion"),
],
visibility = ["//visibility:public"],
)

17
BUILD.bazel Normal file
View file

@ -0,0 +1,17 @@
load("@rules_python//python:defs.bzl", "py_library", "py_binary")
load("@python_deps//:requirements.bzl", "requirement")
py_library(
name = "gello",
srcs = glob(["*.py", "**/*.py"]),
deps = [
requirement("numpy"),
requirement("torchvision"),
requirement("torch"),
requirement("ultralytics"),
requirement("opencv-python"),
requirement("pyquaternion"),
],
visibility = ["//visibility:public"],
)

9
experiments/run_env.py
View file

@ -117,7 +117,7 @@ def main(args):
) )
if args.start_joints is None: if args.start_joints is None:
reset_joints = np.deg2rad( reset_joints = np.deg2rad(
[0, -90, 90, -90, -90, 0, 0] [90, 0, 0, 0, 0, 0, 0, 0]
) # Change this to your own reset joints ) # Change this to your own reset joints
else: else:
reset_joints = args.start_joints reset_joints = args.start_joints
@ -146,10 +146,12 @@ def main(args):
raise ValueError("Invalid agent name") raise ValueError("Invalid agent name")
# going to start position # going to start position
print("Going to start position") print(f"Going to start position {env.get_obs()}")
start_pos = agent.act(env.get_obs()) start_pos = agent.act(env.get_obs())
print(f"start_pos as: {start_pos}" )
obs = env.get_obs() obs = env.get_obs()
joints = obs["joint_positions"] joints = obs["joint_positions"]
print(f"joints {joints}")
abs_deltas = np.abs(start_pos - joints) abs_deltas = np.abs(start_pos - joints)
id_max_joint_delta = np.argmax(abs_deltas) id_max_joint_delta = np.argmax(abs_deltas)
@ -189,7 +191,7 @@ def main(args):
obs = env.get_obs() obs = env.get_obs()
joints = obs["joint_positions"] joints = obs["joint_positions"]
action = agent.act(obs) action = agent.act(obs)
if (action - joints > 0.5).any(): if (action - joints > 0.5*10).any():
print("Action is too big") print("Action is too big")
# print which joints are too big # print which joints are too big
@ -220,6 +222,7 @@ def main(args):
flush=True, flush=True,
) )
action = agent.act(obs) action = agent.act(obs)
print(f"action: {action}")
dt = datetime.datetime.now() dt = datetime.datetime.now()
if args.use_save_interface: if args.use_save_interface:
state = kb_interface.update() state = kb_interface.update()

92
gello/agents/gello_agent.py
View file

@ -6,6 +6,8 @@ import numpy as np
from gello.agents.agent import Agent from gello.agents.agent import Agent
from gello.robots.dynamixel import DynamixelRobot from gello.robots.dynamixel import DynamixelRobot
import time # Import the time module
@dataclass @dataclass
@ -29,9 +31,9 @@ class DynamixelRobotConfig:
assert len(self.joint_ids) == len(self.joint_offsets) assert len(self.joint_ids) == len(self.joint_offsets)
assert len(self.joint_ids) == len(self.joint_signs) assert len(self.joint_ids) == len(self.joint_signs)
def make_robot( def make_robot(self,
self, port: str = "/dev/ttyUSB0", start_joints: Optional[np.ndarray] = None port: str = "/dev/ttyUSB0",
) -> DynamixelRobot: start_joints: Optional[np.ndarray] = None) -> DynamixelRobot:
return DynamixelRobot( return DynamixelRobot(
joint_ids=self.joint_ids, joint_ids=self.joint_ids,
joint_offsets=list(self.joint_offsets), joint_offsets=list(self.joint_offsets),
@ -45,23 +47,27 @@ class DynamixelRobotConfig:
PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = { PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = {
# xArm # xArm
# "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0": DynamixelRobotConfig( "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT94VP8U-if00-port0":
# joint_ids=(1, 2, 3, 4, 5, 6, 7), DynamixelRobotConfig(
# joint_offsets=( joint_ids=(1, 2, 3, 4, 5, 6, 7),
# 2 * np.pi / 2, # 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
# 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
# 2 * np.pi / 2, joint_offsets=(
# 2 * np.pi / 2, 3 * np.pi / 4,
# -1 * np.pi / 2 + 2 * np.pi, 4 * np.pi / 4,
# 1 * np.pi / 2, 14 * np.pi / 4,
# 1 * np.pi / 2, 0 * np.pi / 4,
# ), -4 * np.pi / 4,
# joint_signs=(1, 1, 1, 1, 1, 1, 1), 4 * np.pi / 4,
# gripper_config=(8, 279, 279 - 50), 2 * np.pi / 4,
# ), ),
joint_signs=(1, -1, 1, 1, 1, 1, 1),
gripper_config=(8, 330.79609375, 288.99609375),
),
# panda # panda
# "/dev/cu.usbserial-FT3M9NVB": DynamixelRobotConfig( # "/dev/cu.usbserial-FT3M9NVB": DynamixelRobotConfig(
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0": DynamixelRobotConfig( "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0":
DynamixelRobotConfig(
joint_ids=(1, 2, 3, 4, 5, 6, 7), joint_ids=(1, 2, 3, 4, 5, 6, 7),
joint_offsets=( joint_offsets=(
3 * np.pi / 2, 3 * np.pi / 2,
@ -76,7 +82,8 @@ PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = {
gripper_config=(8, 195, 152), gripper_config=(8, 195, 152),
), ),
# Left UR # Left UR
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBEIA-if00-port0": DynamixelRobotConfig( "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBEIA-if00-port0":
DynamixelRobotConfig(
joint_ids=(1, 2, 3, 4, 5, 6), joint_ids=(1, 2, 3, 4, 5, 6),
joint_offsets=( joint_offsets=(
0, 0,
@ -90,7 +97,8 @@ PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = {
gripper_config=(7, 20, -22), gripper_config=(7, 20, -22),
), ),
# Right UR # Right UR
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBG6A-if00-port0": DynamixelRobotConfig( "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBG6A-if00-port0":
DynamixelRobotConfig(
joint_ids=(1, 2, 3, 4, 5, 6), joint_ids=(1, 2, 3, 4, 5, 6),
joint_offsets=( joint_offsets=(
np.pi + 0 * np.pi, np.pi + 0 * np.pi,
@ -107,6 +115,7 @@ PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = {
class GelloAgent(Agent): class GelloAgent(Agent):
def __init__( def __init__(
self, self,
port: str, port: str,
@ -114,26 +123,53 @@ class GelloAgent(Agent):
start_joints: Optional[np.ndarray] = None, start_joints: Optional[np.ndarray] = None,
): ):
if dynamixel_config is not None: if dynamixel_config is not None:
self._robot = dynamixel_config.make_robot( self._robot = dynamixel_config.make_robot(port=port,
port=port, start_joints=start_joints start_joints=start_joints)
)
else: else:
assert os.path.exists(port), port assert os.path.exists(port), port
assert port in PORT_CONFIG_MAP, f"Port {port} not in config map" assert port in PORT_CONFIG_MAP, f"Port {port} not in config map"
config = PORT_CONFIG_MAP[port] config = PORT_CONFIG_MAP[port]
self._robot = config.make_robot(port=port, start_joints=start_joints) self._robot = config.make_robot(port=port,
start_joints=start_joints)
def act(self, obs: Dict[str, np.ndarray]) -> np.ndarray: def act(self, obs: Dict[str, np.ndarray]) -> np.ndarray:
return self._robot.get_joint_state() # return self._robot.get_joint_state()
dyna_joints = self._robot.get_joint_state() dyna_joints = self._robot.get_joint_state()
# current_q = dyna_joints[:-1] # last one dim is the gripper # current_q = dyna_joints[:-1] # last one dim is the gripper
current_gripper = dyna_joints[-1] # last one dim is the gripper current_gripper = dyna_joints[-1] # last one dim is the gripper
print(current_gripper) # print(current_gripper)
if current_gripper < 0.2: if current_gripper < 0.2:
self._robot.set_torque_mode(False) # self._robot.set_torque_mode(False)
return obs["joint_positions"] return obs["joint_positions"]
else: else:
self._robot.set_torque_mode(False) # self._robot.set_torque_mode(False)
return dyna_joints return dyna_joints
def get_gello_joint_state(self) -> Tuple[np.ndarray, float]:
dyna_joints = self._robot.get_joint_state()
gripper = dyna_joints[-1]
return dyna_joints[:-1], gripper
class FakeGelloAgent(Agent):
def __init__(self):
self.gripper_state = 1.0
self.start_time = time.time() # Record the start time
self.initial_offset = np.array([-1.147, -1.061, 2.261, 1.391, 1.987, 1.392, -2.685])
self.joint_lower_limits = np.full(7, -np.pi/2)
self.joint_upper_limits = np.full(7, np.pi/2)
self.initial_joint_positions = np.random.uniform(self.joint_lower_limits, self.joint_upper_limits)
self.initial_joint_positions = self.initial_joint_positions + self.initial_offset
def act(self, obs: Dict[str, np.ndarray]) -> np.ndarray:
return obs["joint_positions"]
def get_gello_joint_state(self) -> Tuple[np.ndarray, float]:
# Compute the elapsed time since initialization
elapsed_time = time.time() - self.start_time
# Create joint positions that increment over time
joint_positions = self.initial_joint_positions + elapsed_time * 0.1 # Increment each joint by 0.1 radians per second
joint_positions = (joint_positions + np.pi) % (2 * np.pi) - np.pi
return joint_positions, self.gripper_state

98
gello/dynamixel/driver.py
View file

@ -3,6 +3,7 @@ from threading import Event, Lock, Thread
from typing import Protocol, Sequence from typing import Protocol, Sequence
import numpy as np import numpy as np
from dynamixel_sdk.group_sync_read import GroupSyncRead from dynamixel_sdk.group_sync_read import GroupSyncRead
from dynamixel_sdk.group_sync_write import GroupSyncWrite from dynamixel_sdk.group_sync_write import GroupSyncWrite
from dynamixel_sdk.packet_handler import PacketHandler from dynamixel_sdk.packet_handler import PacketHandler
@ -26,6 +27,7 @@ TORQUE_DISABLE = 0
class DynamixelDriverProtocol(Protocol): class DynamixelDriverProtocol(Protocol):
def set_joints(self, joint_angles: Sequence[float]): def set_joints(self, joint_angles: Sequence[float]):
"""Set the joint angles for the Dynamixel servos. """Set the joint angles for the Dynamixel servos.
@ -63,6 +65,7 @@ class DynamixelDriverProtocol(Protocol):
class FakeDynamixelDriver(DynamixelDriverProtocol): class FakeDynamixelDriver(DynamixelDriverProtocol):
def __init__(self, ids: Sequence[int]): def __init__(self, ids: Sequence[int]):
self._ids = ids self._ids = ids
self._joint_angles = np.zeros(len(ids), dtype=int) self._joint_angles = np.zeros(len(ids), dtype=int)
@ -71,8 +74,7 @@ class FakeDynamixelDriver(DynamixelDriverProtocol):
def set_joints(self, joint_angles: Sequence[float]): def set_joints(self, joint_angles: Sequence[float]):
if len(joint_angles) != len(self._ids): if len(joint_angles) != len(self._ids):
raise ValueError( raise ValueError(
"The length of joint_angles must match the number of servos" "The length of joint_angles must match the number of servos")
)
if not self._torque_enabled: if not self._torque_enabled:
raise RuntimeError("Torque must be enabled to set joint angles") raise RuntimeError("Torque must be enabled to set joint angles")
self._joint_angles = np.array(joint_angles) self._joint_angles = np.array(joint_angles)
@ -91,9 +93,11 @@ class FakeDynamixelDriver(DynamixelDriverProtocol):
class DynamixelDriver(DynamixelDriverProtocol): class DynamixelDriver(DynamixelDriverProtocol):
def __init__(
self, ids: Sequence[int], port: str = "/dev/ttyUSB0", baudrate: int = 57600 def __init__(self,
): ids: Sequence[int],
port: str = "/dev/ttyUSB0",
baudrate: int = 57600):
"""Initialize the DynamixelDriver class. """Initialize the DynamixelDriver class.
Args: Args:
@ -132,24 +136,47 @@ class DynamixelDriver(DynamixelDriverProtocol):
for dxl_id in self._ids: for dxl_id in self._ids:
if not self._groupSyncRead.addParam(dxl_id): if not self._groupSyncRead.addParam(dxl_id):
raise RuntimeError( raise RuntimeError(
f"Failed to add parameter for Dynamixel with ID {dxl_id}" f"Failed to add parameter for Dynamixel with ID {dxl_id}")
)
# Disable torque for each Dynamixel servo # Disable torque for each Dynamixel servo
self._torque_enabled = False self._torque_enabled = False
try: # try:
self.set_torque_mode(self._torque_enabled) # self.set_torque_mode(self._torque_enabled)
except Exception as e: # except Exception as e:
print(f"port: {port}, {e}") # print(f"port: {port}, {e}")
self._stop_thread = Event() self._stop_thread = Event()
self._start_reading_thread() self._start_reading_thread()
def move_single_joint(self, dxl_id, joint_angle: float):
if not self._torque_enabled:
raise RuntimeError("Torque must be enabled to set joint angles")
result, error = self._packetHandler.write4ByteTxRx(
self._portHandler, dxl_id, ADDR_GOAL_POSITION,
int(joint_angle * 2048 / np.pi))
if result != COMM_SUCCESS:
print(
f"move_single_joint: {dxl_id}, {joint_angle}, {result}, {error}"
)
# raise RuntimeError(f"Failed to write goal position for Dynamixel with ID {dxl_id}")
def move_joints(self, joint_angles: Sequence[float]):
if len(joint_angles) != len(self._ids):
raise ValueError(
"The length of joint_angles must match the number of servos")
if not self._torque_enabled:
raise RuntimeError("Torque must be enabled to set joint angles")
# Write goal position
for dxl_id, angle in zip(self._ids, joint_angles):
self.move_single_joint(dxl_id, angle)
def set_joints(self, joint_angles: Sequence[float]): def set_joints(self, joint_angles: Sequence[float]):
if len(joint_angles) != len(self._ids): if len(joint_angles) != len(self._ids):
raise ValueError( raise ValueError(
"The length of joint_angles must match the number of servos" "The length of joint_angles must match the number of servos")
)
if not self._torque_enabled: if not self._torque_enabled:
raise RuntimeError("Torque must be enabled to set joint angles") raise RuntimeError("Torque must be enabled to set joint angles")
@ -167,12 +194,10 @@ class DynamixelDriver(DynamixelDriverProtocol):
# Add goal position value to the Syncwrite parameter storage # Add goal position value to the Syncwrite parameter storage
dxl_addparam_result = self._groupSyncWrite.addParam( dxl_addparam_result = self._groupSyncWrite.addParam(
dxl_id, param_goal_position dxl_id, param_goal_position)
)
if not dxl_addparam_result: if not dxl_addparam_result:
raise RuntimeError( raise RuntimeError(
f"Failed to set joint angle for Dynamixel with ID {dxl_id}" f"Failed to set joint angle for Dynamixel with ID {dxl_id}")
)
# Syncwrite goal position # Syncwrite goal position
dxl_comm_result = self._groupSyncWrite.txPacket() dxl_comm_result = self._groupSyncWrite.txPacket()
@ -190,8 +215,7 @@ class DynamixelDriver(DynamixelDriverProtocol):
with self._lock: with self._lock:
for dxl_id in self._ids: for dxl_id in self._ids:
dxl_comm_result, dxl_error = self._packetHandler.write1ByteTxRx( dxl_comm_result, dxl_error = self._packetHandler.write1ByteTxRx(
self._portHandler, dxl_id, ADDR_TORQUE_ENABLE, torque_value self._portHandler, dxl_id, ADDR_TORQUE_ENABLE, torque_value)
)
if dxl_comm_result != COMM_SUCCESS or dxl_error != 0: if dxl_comm_result != COMM_SUCCESS or dxl_error != 0:
print(dxl_comm_result) print(dxl_comm_result)
print(dxl_error) print(dxl_error)
@ -214,15 +238,14 @@ class DynamixelDriver(DynamixelDriverProtocol):
_joint_angles = np.zeros(len(self._ids), dtype=int) _joint_angles = np.zeros(len(self._ids), dtype=int)
dxl_comm_result = self._groupSyncRead.txRxPacket() dxl_comm_result = self._groupSyncRead.txRxPacket()
if dxl_comm_result != COMM_SUCCESS: if dxl_comm_result != COMM_SUCCESS:
print(f"warning, comm failed: {dxl_comm_result}") # print(f"warning, comm failed: {dxl_comm_result}")
continue continue
for i, dxl_id in enumerate(self._ids): for i, dxl_id in enumerate(self._ids):
if self._groupSyncRead.isAvailable( if self._groupSyncRead.isAvailable(dxl_id,
dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION ADDR_PRESENT_POSITION,
): LEN_PRESENT_POSITION):
angle = self._groupSyncRead.getData( angle = self._groupSyncRead.getData(
dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION)
)
angle = np.int32(np.uint32(angle)) angle = np.int32(np.uint32(angle))
_joint_angles[i] = angle _joint_angles[i] = angle
else: else:
@ -248,7 +271,7 @@ class DynamixelDriver(DynamixelDriverProtocol):
def main(): def main():
# Set the port, baudrate, and servo IDs # Set the port, baudrate, and servo IDs
ids = [1] ids = [1, 2, 3, 4, 5, 6, 7]
# Create a DynamixelDriver instance # Create a DynamixelDriver instance
try: try:
@ -256,15 +279,34 @@ def main():
except FileNotFoundError: except FileNotFoundError:
driver = DynamixelDriver(ids, port="/dev/cu.usbserial-FT7WBMUB") driver = DynamixelDriver(ids, port="/dev/cu.usbserial-FT7WBMUB")
joint_angles = driver.get_joints()
print(f"Joint angles for IDs {ids}: {joint_angles}")
# Test setting torque mode # Test setting torque mode
driver.set_torque_mode(True) # driver.set_torque_mode(True)
driver.set_torque_mode(False) # driver.set_torque_mode(False)
# driver.move_single_joint(1, 4)
# driver.move_single_joint(2, 0.2)
# driver.move_single_joint(3, 2)
# driver.move_single_joint(4, 3)
# driver.move_single_joint(5, 1.5)
# driver.move_single_joint(6, 2.5)
# driver.move_single_joint(7, 0.25)
init_angles = np.array([4, 0, 2, 2.5, 1.5, 3, 3.5])
driver.move_joints(init_angles)
# Test reading the joint angles # Test reading the joint angles
try: try:
while True: while True:
joint_angles = driver.get_joints() joint_angles = driver.get_joints()
print(f"Joint angles for IDs {ids}: {joint_angles}") print(f"Joint angles for IDs {ids}: {joint_angles}")
next_joint = joint_angles - 0.05
time.sleep(1.0)
print(f"next_joint: {next_joint}")
driver.move_joints(joint_angles)
time.sleep(5.0)
# print(f"Joint angles for IDs {ids[1]}: {joint_angles[1]}") # print(f"Joint angles for IDs {ids[1]}: {joint_angles[1]}")
except KeyboardInterrupt: except KeyboardInterrupt:
driver.close() driver.close()

8
gello/robots/dynamixel.py
View file

@ -30,6 +30,7 @@ class DynamixelRobot(Robot):
if gripper_config is not None: if gripper_config is not None:
assert joint_offsets is not None assert joint_offsets is not None
assert joint_signs is not None assert joint_signs is not None
print("add gripper...")
# joint_ids.append(gripper_config[0]) # joint_ids.append(gripper_config[0])
# joint_offsets.append(0.0) # joint_offsets.append(0.0)
@ -41,6 +42,7 @@ class DynamixelRobot(Robot):
gripper_config[1] * np.pi / 180, gripper_config[1] * np.pi / 180,
gripper_config[2] * np.pi / 180, gripper_config[2] * np.pi / 180,
) )
print(f"joint_ids: {joint_ids}")
else: else:
self.gripper_open_close = None self.gripper_open_close = None
@ -71,7 +73,7 @@ class DynamixelRobot(Robot):
if real: if real:
self._driver = DynamixelDriver(joint_ids, port=port, baudrate=baudrate) self._driver = DynamixelDriver(joint_ids, port=port, baudrate=baudrate)
self._driver.set_torque_mode(False) # self._driver.set_torque_mode(False)
else: else:
self._driver = FakeDynamixelDriver(joint_ids) self._driver = FakeDynamixelDriver(joint_ids)
self._torque_on = False self._torque_on = False
@ -109,10 +111,13 @@ class DynamixelRobot(Robot):
if self.gripper_open_close is not None: if self.gripper_open_close is not None:
# map pos to [0, 1] # map pos to [0, 1]
# print(f"pos: {pos[-1]}")
# print(f"gripper_open_close: {self.gripper_open_close}")
g_pos = (pos[-1] - self.gripper_open_close[0]) / ( g_pos = (pos[-1] - self.gripper_open_close[0]) / (
self.gripper_open_close[1] - self.gripper_open_close[0] self.gripper_open_close[1] - self.gripper_open_close[0]
) )
g_pos = min(max(0, g_pos), 1) g_pos = min(max(0, g_pos), 1)
# print(f"g_pos: {g_pos}")
pos[-1] = g_pos pos[-1] = g_pos
if self._last_pos is None: if self._last_pos is None:
@ -125,6 +130,7 @@ class DynamixelRobot(Robot):
return pos return pos
def command_joint_state(self, joint_state: np.ndarray) -> None: def command_joint_state(self, joint_state: np.ndarray) -> None:
print(f"Command driver here as {(joint_state + self._joint_offsets).tolist()}")
self._driver.set_joints((joint_state + self._joint_offsets).tolist()) self._driver.set_joints((joint_state + self._joint_offsets).tolist())
def set_torque_mode(self, mode: bool): def set_torque_mode(self, mode: bool):

7
installdeps.sh Executable file
View file

@ -0,0 +1,7 @@
#!/bin/bash
git submodule init
git submodule update
pip install -r requirements.txt
pip install -e .
pip install -e third_party/DynamixelSDK/python

4
scripts/gello_get_offset.py
View file

@ -65,7 +65,7 @@ def get_config(args: Args) -> None:
best_offset = 0 best_offset = 0
best_error = 1e6 best_error = 1e6
for offset in np.linspace( 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 ): # intervals of pi/2
error = get_error(offset, i, curr_joints) error = get_error(offset, i, curr_joints)
if error < best_error: 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 : ", [f"{x:.3f}" for x in best_offsets])
print( print(
"best offsets function of pi: [" "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: if args.gripper: