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_port: int = 6001
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):

50
experiments/run_env.py
View file

@ -117,7 +117,7 @@ def main(args):
)
if args.start_joints is None:
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
else:
reset_joints = args.start_joints
@ -154,7 +154,7 @@ def main(args):
abs_deltas = np.abs(start_pos - joints)
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:
id_mask = abs_deltas > max_joint_delta
print()
@ -175,30 +175,30 @@ def main(args):
joints
), f"agent output dim = {len(start_pos)}, but env dim = {len(joints)}"
max_delta = 0.05
for _ in range(25):
obs = env.get_obs()
command_joints = agent.act(obs)
current_joints = obs["joint_positions"]
delta = command_joints - current_joints
max_joint_delta = np.abs(delta).max()
if max_joint_delta > max_delta:
delta = delta / max_joint_delta * max_delta
env.step(current_joints + delta)
# max_delta = 0.05
# for _ in range(25):
# obs = env.get_obs()
# command_joints = agent.act(obs)
# current_joints = obs["joint_positions"]
# delta = command_joints - current_joints
# max_joint_delta = np.abs(delta).max()
# if max_joint_delta > max_delta:
# delta = delta / max_joint_delta * max_delta
# env.step(current_joints + delta)
obs = env.get_obs()
joints = obs["joint_positions"]
action = agent.act(obs)
if (action - joints > 0.5).any():
print("Action is too big")
# print which joints are too big
joint_index = np.where(action - joints > 0.8)
for j in joint_index:
print(
f"Joint [{j}], leader: {action[j]}, follower: {joints[j]}, diff: {action[j] - joints[j]}"
)
exit()
# obs = env.get_obs()
# joints = obs["joint_positions"]
# action = agent.act(obs)
# if (action - joints > 0.5).any():
# print("Action is too big")
#
# # print which joints are too big
# joint_index = np.where(action - joints > 0.8)
# for j in joint_index:
# print(
# f"Joint [{j}], leader: {action[j]}, follower: {joints[j]}, diff: {action[j] - joints[j]}"
# )
# exit()
if args.use_save_interface:
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
# "/dev/cu.usbserial-FT3M9NVB": DynamixelRobotConfig(
# "/dev/cu.usbserial-FT89FJNO": 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_offsets=(
1 * np.pi / 2,
2 * np.pi / 2,
4 * np.pi / 2,
2 * np.pi / 2,
0 * np.pi / 2,
0 * np.pi / 2,
2 * np.pi / 2,
3 * np.pi / 2,
3 * np.pi / 2,
1 * np.pi / 2,
3 * np.pi / 2 - np.pi/3,
),
joint_signs=(1, -1, 1, 1, 1, -1, 1),
gripper_config=(8, 205, 170),
joint_signs=(1, 1, 1, 1, 1, 1, 1),
gripper_config=(8, 209, 168),
),
# Left UR
"/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):
GRIPPER_OPEN = 800
GRIPPER_CLOSE = 0
GRIPPER_CLOSE = 100
# MAX_DELTA = 0.2
DEFAULT_MAX_DELTA = 0.05
@ -155,9 +155,11 @@ class XArmRobot(Robot):
real: bool = True,
control_frequency: float = 50.0,
max_delta: float = DEFAULT_MAX_DELTA,
use_robotiq: bool = True,
):
print(ip)
self.real = real
self.use_robotiq = use_robotiq
self.max_delta = max_delta
if real:
from xarm.wrapper import XArmAPI
@ -166,6 +168,12 @@ class XArmRobot(Robot):
else:
self.robot = None
if self.use_robotiq:
import pyRobotiqGripper
gripper = pyRobotiqGripper.RobotiqGripper()
self.gripper = gripper
#gripper.activate()
self._control_frequency = control_frequency
self._clear_error_states()
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.start()
def get_state(self) -> RobotState:
with self.last_state_lock:
return self.last_state
@ -207,33 +216,48 @@ class XArmRobot(Robot):
time.sleep(1)
self.robot.set_state(state=0)
time.sleep(1)
self.robot.set_gripper_enable(True)
time.sleep(1)
self.robot.set_gripper_mode(0)
time.sleep(1)
self.robot.set_gripper_speed(3000)
time.sleep(1)
if self.use_robotiq:
pass
else:
self.robot.set_gripper_enable(True)
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:
if self.robot is None:
return 0.0
code, gripper_pos = self.robot.get_gripper_position()
while code != 0 or gripper_pos is None:
print(f"Error code {code} in get_gripper_position(). {gripper_pos}")
time.sleep(0.001)
if self.use_robotiq:
return self.gripper.getPosition() / 255
else:
code, gripper_pos = self.robot.get_gripper_position()
if code == 22:
self._clear_error_states()
while code != 0 or gripper_pos is None:
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) / (
self.GRIPPER_CLOSE - self.GRIPPER_OPEN
)
return normalized_gripper_pos
normalized_gripper_pos = (gripper_pos - self.GRIPPER_OPEN) / (
self.GRIPPER_CLOSE - self.GRIPPER_OPEN
)
return normalized_gripper_pos
def _set_gripper_position(self, pos: int) -> None:
if self.robot is None:
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():
# time.sleep(0.01)
@ -347,12 +371,26 @@ def main():
time.sleep(1)
print(robot.get_state())
print(robot.get_state())
print(robot.get_state())
print(robot.get_state())
print(robot.get_state())
time.sleep(1)
print(robot.get_state())
print("end")
robot.command_joint_state(np.zeros(7))
robot.stop()
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()