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

running

Browse source
This commit is contained in:
Your Name 2024-05-25 23:34:18 -07:00
parent e05749660a
commit bf62f03e06
4 changed files with 91 additions and 51 deletions
Download patch file Download diff file Expand all files Collapse all files

2
experiments/launch_nodes.py
View file

@ -12,7 +12,7 @@ class Args:
robot: str = "xarm" robot: str = "xarm"
robot_port: int = 6001 robot_port: int = 6001
hostname: str = "127.0.0.1" hostname: str = "127.0.0.1"
robot_ip: str = "192.168.1.10" robot_ip: str = "192.168.1.226"
def launch_robot_server(args: Args): def launch_robot_server(args: Args):

50
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, 0, 0, 90, 0, 90, 0, 0] [-90, 0, 270, 90, 0, 90, 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
@ -154,7 +154,7 @@ def main(args):
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)
max_joint_delta = 0.8 max_joint_delta = 0.9
if abs_deltas[id_max_joint_delta] > max_joint_delta: if abs_deltas[id_max_joint_delta] > max_joint_delta:
id_mask = abs_deltas > max_joint_delta id_mask = abs_deltas > max_joint_delta
print() print()
@ -175,30 +175,30 @@ def main(args):
joints joints
), f"agent output dim = {len(start_pos)}, but env dim = {len(joints)}" ), f"agent output dim = {len(start_pos)}, but env dim = {len(joints)}"
max_delta = 0.05 # max_delta = 0.05
for _ in range(25): # for _ in range(25):
obs = env.get_obs() # obs = env.get_obs()
command_joints = agent.act(obs) # command_joints = agent.act(obs)
current_joints = obs["joint_positions"] # current_joints = obs["joint_positions"]
delta = command_joints - current_joints # delta = command_joints - current_joints
max_joint_delta = np.abs(delta).max() # max_joint_delta = np.abs(delta).max()
if max_joint_delta > max_delta: # if max_joint_delta > max_delta:
delta = delta / max_joint_delta * max_delta # delta = delta / max_joint_delta * max_delta
env.step(current_joints + delta) # env.step(current_joints + delta)
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).any():
print("Action is too big") # print("Action is too big")
#
# print which joints are too big # # print which joints are too big
joint_index = np.where(action - joints > 0.8) # joint_index = np.where(action - joints > 0.8)
for j in joint_index: # for j in joint_index:
print( # print(
f"Joint [{j}], leader: {action[j]}, follower: {joints[j]}, diff: {action[j] - joints[j]}" # f"Joint [{j}], leader: {action[j]}, follower: {joints[j]}, diff: {action[j] - joints[j]}"
) # )
exit() # exit()
if args.use_save_interface: if args.use_save_interface:
from gello.data_utils.keyboard_interface import KBReset from gello.data_utils.keyboard_interface import KBReset

16
gello/agents/gello_agent.py
View file

@ -61,20 +61,22 @@ PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = {
# ), # ),
# panda # panda
# "/dev/cu.usbserial-FT3M9NVB": DynamixelRobotConfig( # "/dev/cu.usbserial-FT3M9NVB": DynamixelRobotConfig(
# "/dev/cu.usbserial-FT89FJNO": 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(
"/dev/cu.usbserial-FT89FJNO": DynamixelRobotConfig(
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT89FJNO-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=(
1 * np.pi / 2,
2 * np.pi / 2, 2 * np.pi / 2,
4 * np.pi / 2, 2 * np.pi / 2,
0 * np.pi / 2,
0 * np.pi / 2, 0 * np.pi / 2,
2 * np.pi / 2, 2 * np.pi / 2,
3 * np.pi / 2, 1 * np.pi / 2,
3 * np.pi / 2, 3 * np.pi / 2 - np.pi/3,
), ),
joint_signs=(1, -1, 1, 1, 1, -1, 1), joint_signs=(1, 1, 1, 1, 1, 1, 1),
gripper_config=(8, 205, 170), gripper_config=(8, 209, 168),
), ),
# 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(

74
gello/robots/xarm_robot.py
View file

@ -118,7 +118,7 @@ class Rate:
class XArmRobot(Robot): class XArmRobot(Robot):
GRIPPER_OPEN = 800 GRIPPER_OPEN = 800
GRIPPER_CLOSE = 0 GRIPPER_CLOSE = 100
# MAX_DELTA = 0.2 # MAX_DELTA = 0.2
DEFAULT_MAX_DELTA = 0.05 DEFAULT_MAX_DELTA = 0.05
@ -155,9 +155,11 @@ class XArmRobot(Robot):
real: bool = True, real: bool = True,
control_frequency: float = 50.0, control_frequency: float = 50.0,
max_delta: float = DEFAULT_MAX_DELTA, max_delta: float = DEFAULT_MAX_DELTA,
use_robotiq: bool = True,
): ):
print(ip) print(ip)
self.real = real self.real = real
self.use_robotiq = use_robotiq
self.max_delta = max_delta self.max_delta = max_delta
if real: if real:
from xarm.wrapper import XArmAPI from xarm.wrapper import XArmAPI
@ -166,6 +168,12 @@ class XArmRobot(Robot):
else: else:
self.robot = None self.robot = None
if self.use_robotiq:
import pyRobotiqGripper
gripper = pyRobotiqGripper.RobotiqGripper()
self.gripper = gripper
#gripper.activate()
self._control_frequency = control_frequency self._control_frequency = control_frequency
self._clear_error_states() self._clear_error_states()
self._set_gripper_position(self.GRIPPER_OPEN) self._set_gripper_position(self.GRIPPER_OPEN)
@ -183,6 +191,7 @@ class XArmRobot(Robot):
self.command_thread = threading.Thread(target=self._robot_thread) self.command_thread = threading.Thread(target=self._robot_thread)
self.command_thread.start() self.command_thread.start()
def get_state(self) -> RobotState: def get_state(self) -> RobotState:
with self.last_state_lock: with self.last_state_lock:
return self.last_state return self.last_state
@ -207,33 +216,48 @@ class XArmRobot(Robot):
time.sleep(1) time.sleep(1)
self.robot.set_state(state=0) self.robot.set_state(state=0)
time.sleep(1) time.sleep(1)
self.robot.set_gripper_enable(True) if self.use_robotiq:
time.sleep(1) pass
self.robot.set_gripper_mode(0) else:
time.sleep(1) self.robot.set_gripper_enable(True)
self.robot.set_gripper_speed(3000) time.sleep(1)
time.sleep(1) self.robot.set_gripper_mode(0)
time.sleep(1)
self.robot.set_gripper_speed(3000)
time.sleep(1)
def _get_gripper_pos(self) -> float: def _get_gripper_pos(self) -> float:
if self.robot is None: if self.robot is None:
return 0.0 return 0.0
code, gripper_pos = self.robot.get_gripper_position() if self.use_robotiq:
while code != 0 or gripper_pos is None: return self.gripper.getPosition() / 255
print(f"Error code {code} in get_gripper_position(). {gripper_pos}") else:
time.sleep(0.001)
code, gripper_pos = self.robot.get_gripper_position() code, gripper_pos = self.robot.get_gripper_position()
if code == 22: while code != 0 or gripper_pos is None:
self._clear_error_states() print(f"Error code {code} in get_gripper_position(). {gripper_pos}")
time.sleep(0.001)
code, gripper_pos = self.robot.get_gripper_position()
if code == 22:
self._clear_error_states()
normalized_gripper_pos = (gripper_pos - self.GRIPPER_OPEN) / ( normalized_gripper_pos = (gripper_pos - self.GRIPPER_OPEN) / (
self.GRIPPER_CLOSE - self.GRIPPER_OPEN self.GRIPPER_CLOSE - self.GRIPPER_OPEN
) )
return normalized_gripper_pos return normalized_gripper_pos
def _set_gripper_position(self, pos: int) -> None: def _set_gripper_position(self, pos: int) -> None:
if self.robot is None: if self.robot is None:
return return
self.robot.set_gripper_position(pos, wait=False) if self.use_robotiq:
pos = 255 - (pos / 800 * 255)
try:
self.gripper.goTo(pos, wait=False)
except Exception as e:
print(e)
print(pos)
raise e
else:
self.robot.set_gripper_position(pos, wait=False)
# while self.robot.get_is_moving(): # while self.robot.get_is_moving():
# time.sleep(0.01) # time.sleep(0.01)
@ -347,12 +371,26 @@ def main():
time.sleep(1) time.sleep(1)
print(robot.get_state()) print(robot.get_state())
print(robot.get_state())
print(robot.get_state())
print(robot.get_state())
print(robot.get_state())
time.sleep(1) time.sleep(1)
print(robot.get_state()) print(robot.get_state())
print("end") print("end")
robot.command_joint_state(np.zeros(7))
robot.stop() robot.stop()
if __name__ == "__main__": if __name__ == "__main__":
ip = "192.168.1.226"
# from xarm.wrapper import XArmAPI
# robot = XArmAPI(ip, is_radian=True)
# robot.set_mode(1)
# robot.set_mode(1)
# robot.set_mode(1)
# print(robot.get_servo_angle())
# robot.set_servo_angle_j(np.ones(7)*0.1 + robot.get_servo_angle()[1])
# print(robot.get_state())
main() main()