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

Fake controller

Browse source
This commit is contained in:
Yuwei Xia 2024-09-27 11:01:32 -07:00
parent 8e84ad0222
commit 3fe1373edb
2 changed files with 35 additions and 34 deletions
Download patch file Download diff file Expand all files Collapse all files

6
gello/agents/gello_agent.py
View file

@ -147,3 +147,9 @@ class GelloAgent(Agent):
dyna_joints = self._robot.get_joint_state() dyna_joints = self._robot.get_joint_state()
gripper = dyna_joints[-1] gripper = dyna_joints[-1]
return dyna_joints[:-1], gripper return dyna_joints[:-1], gripper
class FakeGelloAgent(Agent):
def __init__(self):
self.gripper_state = 0.0

57
gello/dynamixel/driver.py
View file

@ -26,6 +26,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 +64,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 +73,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 +92,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,8 +135,7 @@ 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
@ -150,20 +152,19 @@ class DynamixelDriver(DynamixelDriverProtocol):
raise RuntimeError("Torque must be enabled to set joint angles") raise RuntimeError("Torque must be enabled to set joint angles")
result, error = self._packetHandler.write4ByteTxRx( result, error = self._packetHandler.write4ByteTxRx(
self._portHandler, dxl_id, ADDR_GOAL_POSITION, int(joint_angle * 2048 / np.pi) self._portHandler, dxl_id, ADDR_GOAL_POSITION,
) int(joint_angle * 2048 / np.pi))
if result != COMM_SUCCESS: if result != COMM_SUCCESS:
print(f"move_single_joint: {dxl_id}, {joint_angle}, {result}, {error}") 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}") # raise RuntimeError(f"Failed to write goal position for Dynamixel with ID {dxl_id}")
def move_joints(self, joint_angles: Sequence[float]): def move_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")
# Write goal position # Write goal position
@ -171,12 +172,10 @@ class DynamixelDriver(DynamixelDriverProtocol):
for dxl_id, angle in zip(self._ids, joint_angles): for dxl_id, angle in zip(self._ids, joint_angles):
self.move_single_joint(dxl_id, angle) 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")
@ -194,12 +193,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()
@ -217,8 +214,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)
@ -241,15 +237,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:
@ -275,7 +270,7 @@ class DynamixelDriver(DynamixelDriverProtocol):
def main(): def main():
# Set the port, baudrate, and servo IDs # Set the port, baudrate, and servo IDs
ids = [1,2,3,4,5,6,7] ids = [1, 2, 3, 4, 5, 6, 7]
# Create a DynamixelDriver instance # Create a DynamixelDriver instance
try: try:
@ -298,7 +293,7 @@ def main():
# driver.move_single_joint(6, 2.5) # driver.move_single_joint(6, 2.5)
# driver.move_single_joint(7, 0.25) # driver.move_single_joint(7, 0.25)
init_angles = np.array([4,0,2,2.5,1.5,3,3.5]) init_angles = np.array([4, 0, 2, 2.5, 1.5, 3, 3.5])
driver.move_joints(init_angles) driver.move_joints(init_angles)
# Test reading the joint angles # Test reading the joint angles