Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

UPDATE emancro gello #21

Open
wants to merge 6 commits into
base: main
Choose a base branch
from
Open

UPDATE emancro gello #21

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
8e84ad0
UPDATE emancro gello
yuwei-will-xia Sep 23, 2024
3fe1373
Fake controller
yuwei-will-xia Sep 27, 2024
7dad73f
Update dynamixel_sdk
emfebert Sep 27, 2024
ca12950
update fake controller with getjs
yuwei-will-xia Oct 22, 2024
77b8f16
Deprecate torque mode
yuwei-will-xia Oct 29, 2024
a14465a
update js offset
yuwei-will-xia Oct 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 17 additions & 0 deletions .bkpBUILD_bak.bazel
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 changes: 17 additions & 0 deletions BUILD.bazel
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 changes: 6 additions & 3 deletions experiments/run_env.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,7 +117,7 @@ def main(args):
)
if args.start_joints is None:
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
else:
reset_joints = args.start_joints
Expand Down Expand Up @@ -146,10 +146,12 @@ def main(args):
raise ValueError("Invalid agent name")

# 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())
print(f"start_pos as: {start_pos}" )
obs = env.get_obs()
joints = obs["joint_positions"]
print(f"joints {joints}")

abs_deltas = np.abs(start_pos - joints)
id_max_joint_delta = np.argmax(abs_deltas)
Expand Down Expand Up @@ -189,7 +191,7 @@ def main(args):
obs = env.get_obs()
joints = obs["joint_positions"]
action = agent.act(obs)
if (action - joints > 0.5).any():
if (action - joints > 0.5*10).any():
print("Action is too big")

# print which joints are too big
Expand Down Expand Up @@ -220,6 +222,7 @@ def main(args):
flush=True,
)
action = agent.act(obs)
print(f"action: {action}")
dt = datetime.datetime.now()
if args.use_save_interface:
state = kb_interface.update()
Expand Down
158 changes: 96 additions & 62 deletions gello/agents/gello_agent.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@

from gello.agents.agent import Agent
from gello.robots.dynamixel import DynamixelRobot
import time # Import the time module



@dataclass
Expand All @@ -29,9 +31,9 @@ def __post_init__(self):
assert len(self.joint_ids) == len(self.joint_offsets)
assert len(self.joint_ids) == len(self.joint_signs)

def make_robot(
self, port: str = "/dev/ttyUSB0", start_joints: Optional[np.ndarray] = None
) -> DynamixelRobot:
def make_robot(self,
port: str = "/dev/ttyUSB0",
start_joints: Optional[np.ndarray] = None) -> DynamixelRobot:
return DynamixelRobot(
joint_ids=self.joint_ids,
joint_offsets=list(self.joint_offsets),
Expand All @@ -45,95 +47,127 @@ def make_robot(

PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = {
# xArm
# "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0": DynamixelRobotConfig(
# joint_ids=(1, 2, 3, 4, 5, 6, 7),
# joint_offsets=(
# 2 * np.pi / 2,
# 2 * np.pi / 2,
# 2 * np.pi / 2,
# 2 * np.pi / 2,
# -1 * np.pi / 2 + 2 * np.pi,
# 1 * np.pi / 2,
# 1 * np.pi / 2,
# ),
# joint_signs=(1, 1, 1, 1, 1, 1, 1),
# gripper_config=(8, 279, 279 - 50),
# ),
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT94VP8U-if00-port0":
DynamixelRobotConfig(
joint_ids=(1, 2, 3, 4, 5, 6, 7),
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,
),
joint_signs=(1, -1, 1, 1, 1, 1, 1),
gripper_config=(8, 115.024609375, 73.224609375),
),
# panda
# "/dev/cu.usbserial-FT3M9NVB": DynamixelRobotConfig(
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0": DynamixelRobotConfig(
joint_ids=(1, 2, 3, 4, 5, 6, 7),
joint_offsets=(
3 * np.pi / 2,
2 * np.pi / 2,
1 * np.pi / 2,
4 * np.pi / 2,
-2 * np.pi / 2 + 2 * np.pi,
3 * np.pi / 2,
4 * np.pi / 2,
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0":
DynamixelRobotConfig(
joint_ids=(1, 2, 3, 4, 5, 6, 7),
joint_offsets=(
3 * np.pi / 2,
2 * np.pi / 2,
1 * np.pi / 2,
4 * np.pi / 2,
-2 * np.pi / 2 + 2 * np.pi,
3 * np.pi / 2,
4 * np.pi / 2,
),
joint_signs=(1, -1, 1, 1, 1, -1, 1),
gripper_config=(8, 195, 152),
),
joint_signs=(1, -1, 1, 1, 1, -1, 1),
gripper_config=(8, 195, 152),
),
# Left UR
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBEIA-if00-port0": DynamixelRobotConfig(
joint_ids=(1, 2, 3, 4, 5, 6),
joint_offsets=(
0,
1 * np.pi / 2 + np.pi,
np.pi / 2 + 0 * np.pi,
0 * np.pi + np.pi / 2,
np.pi - 2 * np.pi / 2,
-1 * np.pi / 2 + 2 * np.pi,
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBEIA-if00-port0":
DynamixelRobotConfig(
joint_ids=(1, 2, 3, 4, 5, 6),
joint_offsets=(
0,
1 * np.pi / 2 + np.pi,
np.pi / 2 + 0 * np.pi,
0 * np.pi + np.pi / 2,
np.pi - 2 * np.pi / 2,
-1 * np.pi / 2 + 2 * np.pi,
),
joint_signs=(1, 1, -1, 1, 1, 1),
gripper_config=(7, 20, -22),
),
joint_signs=(1, 1, -1, 1, 1, 1),
gripper_config=(7, 20, -22),
),
# Right UR
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBG6A-if00-port0": DynamixelRobotConfig(
joint_ids=(1, 2, 3, 4, 5, 6),
joint_offsets=(
np.pi + 0 * np.pi,
2 * np.pi + np.pi / 2,
2 * np.pi + np.pi / 2,
2 * np.pi + np.pi / 2,
1 * np.pi,
3 * np.pi / 2,
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBG6A-if00-port0":
DynamixelRobotConfig(
joint_ids=(1, 2, 3, 4, 5, 6),
joint_offsets=(
np.pi + 0 * np.pi,
2 * np.pi + np.pi / 2,
2 * np.pi + np.pi / 2,
2 * np.pi + np.pi / 2,
1 * np.pi,
3 * np.pi / 2,
),
joint_signs=(1, 1, -1, 1, 1, 1),
gripper_config=(7, 286, 248),
),
joint_signs=(1, 1, -1, 1, 1, 1),
gripper_config=(7, 286, 248),
),
}


class GelloAgent(Agent):

def __init__(
self,
port: str,
dynamixel_config: Optional[DynamixelRobotConfig] = None,
start_joints: Optional[np.ndarray] = None,
):
if dynamixel_config is not None:
self._robot = dynamixel_config.make_robot(
port=port, start_joints=start_joints
)
self._robot = dynamixel_config.make_robot(port=port,
start_joints=start_joints)
else:
assert os.path.exists(port), port
assert port in PORT_CONFIG_MAP, f"Port {port} not in config map"

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:
return self._robot.get_joint_state()
# return self._robot.get_joint_state()
dyna_joints = self._robot.get_joint_state()
# current_q = 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:
self._robot.set_torque_mode(False)
# self._robot.set_torque_mode(False)
return obs["joint_positions"]
else:
self._robot.set_torque_mode(False)
# self._robot.set_torque_mode(False)
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
Loading