forked from tangger/lerobot
Remove names
This commit is contained in:
@@ -175,13 +175,13 @@ class DynamixelMotorsBus(MotorsBus):
|
||||
drive_modes = self.sync_read("Drive_Mode", normalize=False)
|
||||
|
||||
calibration = {}
|
||||
for name, motor in self.motors.items():
|
||||
calibration[name] = MotorCalibration(
|
||||
id=motor.id,
|
||||
drive_mode=drive_modes[name],
|
||||
homing_offset=offsets[name],
|
||||
range_min=mins[name],
|
||||
range_max=maxes[name],
|
||||
for motor, m in self.motors.items():
|
||||
calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=drive_modes[motor],
|
||||
homing_offset=offsets[motor],
|
||||
range_min=mins[motor],
|
||||
range_max=maxes[motor],
|
||||
)
|
||||
|
||||
return calibration
|
||||
@@ -195,16 +195,16 @@ class DynamixelMotorsBus(MotorsBus):
|
||||
self.calibration = calibration_dict
|
||||
|
||||
def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
|
||||
for name in self._get_motors_list(motors):
|
||||
self.write("Torque_Enable", name, TorqueMode.DISABLED.value, num_retry=num_retry)
|
||||
for motor in self._get_motors_list(motors):
|
||||
self.write("Torque_Enable", motor, TorqueMode.DISABLED.value, num_retry=num_retry)
|
||||
|
||||
def _disable_torque(self, motor_id: int, model: str, num_retry: int = 0) -> None:
|
||||
addr, length = get_address(self.model_ctrl_table, model, "Torque_Enable")
|
||||
self._write(addr, length, motor_id, TorqueMode.DISABLED.value, num_retry=num_retry)
|
||||
|
||||
def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
|
||||
for name in self._get_motors_list(motors):
|
||||
self.write("Torque_Enable", name, TorqueMode.ENABLED.value, num_retry=num_retry)
|
||||
for motor in self._get_motors_list(motors):
|
||||
self.write("Torque_Enable", motor, TorqueMode.ENABLED.value, num_retry=num_retry)
|
||||
|
||||
def _encode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
|
||||
for id_ in ids_values:
|
||||
|
||||
@@ -243,13 +243,13 @@ class FeetechMotorsBus(MotorsBus):
|
||||
# TODO(aliberts): add set/get_drive_mode?
|
||||
|
||||
calibration = {}
|
||||
for name, motor in self.motors.items():
|
||||
calibration[name] = MotorCalibration(
|
||||
id=motor.id,
|
||||
drive_mode=drive_modes[name],
|
||||
homing_offset=offsets[name],
|
||||
range_min=mins[name],
|
||||
range_max=maxes[name],
|
||||
for motor, m in self.motors.items():
|
||||
calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=drive_modes[motor],
|
||||
homing_offset=offsets[motor],
|
||||
range_min=mins[motor],
|
||||
range_max=maxes[motor],
|
||||
)
|
||||
|
||||
return calibration
|
||||
@@ -277,9 +277,9 @@ class FeetechMotorsBus(MotorsBus):
|
||||
return half_turn_homings
|
||||
|
||||
def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
|
||||
for name in self._get_motors_list(motors):
|
||||
self.write("Torque_Enable", name, TorqueMode.DISABLED.value, num_retry=num_retry)
|
||||
self.write("Lock", name, 0, num_retry=num_retry)
|
||||
for motor in self._get_motors_list(motors):
|
||||
self.write("Torque_Enable", motor, TorqueMode.DISABLED.value, num_retry=num_retry)
|
||||
self.write("Lock", motor, 0, num_retry=num_retry)
|
||||
|
||||
def _disable_torque(self, motor_id: int, model: str, num_retry: int = 0) -> None:
|
||||
addr, length = get_address(self.model_ctrl_table, model, "Torque_Enable")
|
||||
@@ -288,9 +288,9 @@ class FeetechMotorsBus(MotorsBus):
|
||||
self._write(addr, length, motor_id, 0, num_retry=num_retry)
|
||||
|
||||
def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
|
||||
for name in self._get_motors_list(motors):
|
||||
self.write("Torque_Enable", name, TorqueMode.ENABLED.value, num_retry=num_retry)
|
||||
self.write("Lock", name, 1, num_retry=num_retry)
|
||||
for motor in self._get_motors_list(motors):
|
||||
self.write("Torque_Enable", motor, TorqueMode.ENABLED.value, num_retry=num_retry)
|
||||
self.write("Lock", motor, 1, num_retry=num_retry)
|
||||
|
||||
def _encode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
|
||||
for id_ in ids_values:
|
||||
|
||||
@@ -282,7 +282,7 @@ class MotorsBus(abc.ABC):
|
||||
self._no_error: int
|
||||
|
||||
self._id_to_model_dict = {m.id: m.model for m in self.motors.values()}
|
||||
self._id_to_name_dict = {m.id: name for name, m in self.motors.items()}
|
||||
self._id_to_name_dict = {m.id: motor for motor, m in self.motors.items()}
|
||||
self._model_nb_to_model_dict = {v: k for k, v in self.model_number_table.items()}
|
||||
|
||||
self._validate_motors()
|
||||
@@ -308,10 +308,6 @@ class MotorsBus(abc.ABC):
|
||||
DeepDiff(first_table, get_ctrl_table(self.model_ctrl_table, model)) for model in self.models[1:]
|
||||
)
|
||||
|
||||
@cached_property
|
||||
def names(self) -> list[str]:
|
||||
return list(self.motors)
|
||||
|
||||
@cached_property
|
||||
def models(self) -> list[str]:
|
||||
return [m.model for m in self.motors.values()]
|
||||
@@ -347,7 +343,7 @@ class MotorsBus(abc.ABC):
|
||||
|
||||
def _get_motors_list(self, motors: str | list[str] | None) -> list[str]:
|
||||
if motors is None:
|
||||
return self.names
|
||||
return list(self.motors)
|
||||
elif isinstance(motors, str):
|
||||
return [motors]
|
||||
elif isinstance(motors, list):
|
||||
@@ -535,7 +531,7 @@ class MotorsBus(abc.ABC):
|
||||
|
||||
def reset_calibration(self, motors: NameOrID | list[NameOrID] | None = None) -> None:
|
||||
if motors is None:
|
||||
motors = self.names
|
||||
motors = list(self.motors)
|
||||
elif isinstance(motors, (str, int)):
|
||||
motors = [motors]
|
||||
elif not isinstance(motors, list):
|
||||
@@ -571,7 +567,7 @@ class MotorsBus(abc.ABC):
|
||||
=> Homing_Offset = ±(X - 2048)
|
||||
"""
|
||||
if motors is None:
|
||||
motors = self.names
|
||||
motors = list(self.motors)
|
||||
elif isinstance(motors, (str, int)):
|
||||
motors = [motors]
|
||||
else:
|
||||
@@ -598,7 +594,7 @@ class MotorsBus(abc.ABC):
|
||||
typically be called prior to this.
|
||||
"""
|
||||
if motors is None:
|
||||
motors = self.names
|
||||
motors = list(self.motors)
|
||||
elif isinstance(motors, (str, int)):
|
||||
motors = [motors]
|
||||
elif not isinstance(motors, list):
|
||||
@@ -615,8 +611,8 @@ class MotorsBus(abc.ABC):
|
||||
if display_values:
|
||||
print("\n-------------------------------------------")
|
||||
print(f"{'NAME':<15} | {'MIN':>6} | {'POS':>6} | {'MAX':>6}")
|
||||
for name in motors:
|
||||
print(f"{name:<15} | {mins[name]:>6} | {positions[name]:>6} | {maxes[name]:>6}")
|
||||
for motor in motors:
|
||||
print(f"{motor:<15} | {mins[motor]:>6} | {positions[motor]:>6} | {maxes[motor]:>6}")
|
||||
|
||||
if enter_pressed():
|
||||
break
|
||||
@@ -633,13 +629,13 @@ class MotorsBus(abc.ABC):
|
||||
|
||||
normalized_values = {}
|
||||
for id_, val in ids_values.items():
|
||||
name = self._id_to_name(id_)
|
||||
min_ = self.calibration[name].range_min
|
||||
max_ = self.calibration[name].range_max
|
||||
motor = self._id_to_name(id_)
|
||||
min_ = self.calibration[motor].range_min
|
||||
max_ = self.calibration[motor].range_max
|
||||
bounded_val = min(max_, max(min_, val))
|
||||
if self.motors[name].norm_mode is MotorNormMode.RANGE_M100_100:
|
||||
if self.motors[motor].norm_mode is MotorNormMode.RANGE_M100_100:
|
||||
normalized_values[id_] = (((bounded_val - min_) / (max_ - min_)) * 200) - 100
|
||||
elif self.motors[name].norm_mode is MotorNormMode.RANGE_0_100:
|
||||
elif self.motors[motor].norm_mode is MotorNormMode.RANGE_0_100:
|
||||
normalized_values[id_] = ((bounded_val - min_) / (max_ - min_)) * 100
|
||||
else:
|
||||
# TODO(alibers): velocity and degree modes
|
||||
@@ -653,13 +649,13 @@ class MotorsBus(abc.ABC):
|
||||
|
||||
unnormalized_values = {}
|
||||
for id_, val in ids_values.items():
|
||||
name = self._id_to_name(id_)
|
||||
min_ = self.calibration[name].range_min
|
||||
max_ = self.calibration[name].range_max
|
||||
if self.motors[name].norm_mode is MotorNormMode.RANGE_M100_100:
|
||||
motor = self._id_to_name(id_)
|
||||
min_ = self.calibration[motor].range_min
|
||||
max_ = self.calibration[motor].range_max
|
||||
if self.motors[motor].norm_mode is MotorNormMode.RANGE_M100_100:
|
||||
bounded_val = min(100.0, max(-100.0, val))
|
||||
unnormalized_values[id_] = int(((bounded_val + 100) / 200) * (max_ - min_) + min_)
|
||||
elif self.motors[name].norm_mode is MotorNormMode.RANGE_0_100:
|
||||
elif self.motors[motor].norm_mode is MotorNormMode.RANGE_0_100:
|
||||
bounded_val = min(100.0, max(0.0, val))
|
||||
unnormalized_values[id_] = int((bounded_val / 100) * (max_ - min_) + min_)
|
||||
else:
|
||||
@@ -854,8 +850,8 @@ class MotorsBus(abc.ABC):
|
||||
self._assert_protocol_is_compatible("sync_read")
|
||||
|
||||
names = self._get_motors_list(motors)
|
||||
ids = [self.motors[name].id for name in names]
|
||||
models = [self.motors[name].model for name in names]
|
||||
ids = [self.motors[motor].id for motor in names]
|
||||
models = [self.motors[motor].model for motor in names]
|
||||
|
||||
if self._has_different_ctrl_tables:
|
||||
assert_same_address(self.model_ctrl_table, models, data_name)
|
||||
|
||||
@@ -114,31 +114,31 @@ class KochFollower(Robot):
|
||||
def calibrate(self) -> None:
|
||||
logger.info(f"\nRunning calibration of {self}")
|
||||
self.arm.disable_torque()
|
||||
for name in self.arm.names:
|
||||
self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
|
||||
for motor in self.arm.motors:
|
||||
self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
|
||||
|
||||
input(f"Move {self} to the middle of its range of motion and press ENTER....")
|
||||
homing_offsets = self.arm.set_half_turn_homings()
|
||||
|
||||
full_turn_motors = ["shoulder_pan", "wrist_roll"]
|
||||
unknown_range_motors = [name for name in self.arm.names if name not in full_turn_motors]
|
||||
unknown_range_motors = [motor for motor in self.arm.motors if motor not in full_turn_motors]
|
||||
print(
|
||||
f"Move all joints except {full_turn_motors} sequentially through their entire "
|
||||
"ranges of motion.\nRecording positions. Press ENTER to stop..."
|
||||
)
|
||||
range_mins, range_maxes = self.arm.record_ranges_of_motion(unknown_range_motors)
|
||||
for name in full_turn_motors:
|
||||
range_mins[name] = 0
|
||||
range_maxes[name] = 4095
|
||||
for motor in full_turn_motors:
|
||||
range_mins[motor] = 0
|
||||
range_maxes[motor] = 4095
|
||||
|
||||
self.calibration = {}
|
||||
for name, motor in self.arm.motors.items():
|
||||
self.calibration[name] = MotorCalibration(
|
||||
id=motor.id,
|
||||
for motor, m in self.arm.motors.items():
|
||||
self.calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=0,
|
||||
homing_offset=homing_offsets[name],
|
||||
range_min=range_mins[name],
|
||||
range_max=range_maxes[name],
|
||||
homing_offset=homing_offsets[motor],
|
||||
range_min=range_mins[motor],
|
||||
range_max=range_maxes[motor],
|
||||
)
|
||||
|
||||
self.arm.write_calibration(self.calibration)
|
||||
@@ -151,9 +151,9 @@ class KochFollower(Robot):
|
||||
# Use 'extended position mode' for all motors except gripper, because in joint mode the servos
|
||||
# can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling
|
||||
# the arm, you could end up with a servo with a position 0 or 4095 at a crucial point
|
||||
for name in self.arm.names:
|
||||
if name != "gripper":
|
||||
self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
|
||||
for motor in self.arm.motors:
|
||||
if motor != "gripper":
|
||||
self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
|
||||
|
||||
# Use 'position control current based' for gripper to be limited by the limit of the current. For
|
||||
# the follower gripper, it means it can grasp an object without forcing too much even tho, its
|
||||
|
||||
@@ -113,14 +113,14 @@ class SO100Follower(Robot):
|
||||
def calibrate(self) -> None:
|
||||
logger.info(f"\nRunning calibration of {self}")
|
||||
self.arm.disable_torque()
|
||||
for name in self.arm.names:
|
||||
self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value)
|
||||
for motor in self.arm.motors:
|
||||
self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
|
||||
|
||||
input(f"Move {self} to the middle of its range of motion and press ENTER....")
|
||||
homing_offsets = self.arm.set_half_turn_homings()
|
||||
|
||||
full_turn_motor = "wrist_roll"
|
||||
unknown_range_motors = [name for name in self.arm.names if name != full_turn_motor]
|
||||
unknown_range_motors = [motor for motor in self.arm.motors if motor != full_turn_motor]
|
||||
print(
|
||||
f"Move all joints except '{full_turn_motor}' sequentially through their "
|
||||
"entire ranges of motion.\nRecording positions. Press ENTER to stop..."
|
||||
@@ -130,13 +130,13 @@ class SO100Follower(Robot):
|
||||
range_maxes[full_turn_motor] = 4095
|
||||
|
||||
self.calibration = {}
|
||||
for name, motor in self.arm.motors.items():
|
||||
self.calibration[name] = MotorCalibration(
|
||||
id=motor.id,
|
||||
for motor, m in self.arm.motors.items():
|
||||
self.calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=0,
|
||||
homing_offset=homing_offsets[name],
|
||||
range_min=range_mins[name],
|
||||
range_max=range_maxes[name],
|
||||
homing_offset=homing_offsets[motor],
|
||||
range_min=range_mins[motor],
|
||||
range_max=range_maxes[motor],
|
||||
)
|
||||
|
||||
self.arm.write_calibration(self.calibration)
|
||||
@@ -146,13 +146,13 @@ class SO100Follower(Robot):
|
||||
def configure(self) -> None:
|
||||
with self.arm.torque_disabled():
|
||||
self.arm.configure_motors()
|
||||
for name in self.arm.names:
|
||||
self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value)
|
||||
for motor in self.arm.motors:
|
||||
self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
|
||||
# Set P_Coefficient to lower value to avoid shakiness (Default is 32)
|
||||
self.arm.write("P_Coefficient", name, 16)
|
||||
self.arm.write("P_Coefficient", motor, 16)
|
||||
# Set I_Coefficient and D_Coefficient to default value 0 and 32
|
||||
self.arm.write("I_Coefficient", name, 0)
|
||||
self.arm.write("D_Coefficient", name, 32)
|
||||
self.arm.write("I_Coefficient", motor, 0)
|
||||
self.arm.write("D_Coefficient", motor, 32)
|
||||
|
||||
def setup_motors(self) -> None:
|
||||
for motor in reversed(self.arm.motors):
|
||||
|
||||
@@ -109,31 +109,31 @@ class ViperX(Robot):
|
||||
raise NotImplementedError # TODO(aliberts): adapt code below (copied from koch
|
||||
logger.info(f"\nRunning calibration of {self}")
|
||||
self.arm.disable_torque()
|
||||
for name in self.arm.names:
|
||||
self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
|
||||
for motor in self.arm.motors:
|
||||
self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
|
||||
|
||||
input("Move robot to the middle of its range of motion and press ENTER....")
|
||||
homing_offsets = self.arm.set_half_turn_homings()
|
||||
|
||||
full_turn_motors = ["shoulder_pan", "wrist_roll"]
|
||||
unknown_range_motors = [name for name in self.arm.names if name not in full_turn_motors]
|
||||
unknown_range_motors = [motor for motor in self.arm.motors if motor not in full_turn_motors]
|
||||
print(
|
||||
f"Move all joints except {full_turn_motors} sequentially through their entire "
|
||||
"ranges of motion.\nRecording positions. Press ENTER to stop..."
|
||||
)
|
||||
range_mins, range_maxes = self.arm.record_ranges_of_motion(unknown_range_motors)
|
||||
for name in full_turn_motors:
|
||||
range_mins[name] = 0
|
||||
range_maxes[name] = 4095
|
||||
for motor in full_turn_motors:
|
||||
range_mins[motor] = 0
|
||||
range_maxes[motor] = 4095
|
||||
|
||||
self.calibration = {}
|
||||
for name, motor in self.arm.motors.items():
|
||||
self.calibration[name] = MotorCalibration(
|
||||
id=motor.id,
|
||||
for motor, m in self.arm.motors.items():
|
||||
self.calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=0,
|
||||
homing_offset=homing_offsets[name],
|
||||
range_min=range_mins[name],
|
||||
range_max=range_maxes[name],
|
||||
homing_offset=homing_offsets[motor],
|
||||
range_min=range_mins[motor],
|
||||
range_max=range_maxes[motor],
|
||||
)
|
||||
|
||||
self.arm.write_calibration(self.calibration)
|
||||
@@ -158,9 +158,9 @@ class ViperX(Robot):
|
||||
# can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling
|
||||
# the arm, you could end up with a servo with a position 0 or 4095 at a crucial point.
|
||||
# See: https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11
|
||||
for name in self.arm.names:
|
||||
if name != "gripper":
|
||||
self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
|
||||
for motor in self.arm.motors:
|
||||
if motor != "gripper":
|
||||
self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
|
||||
|
||||
# Use 'position control current based' for follower gripper to be limited by the limit of the
|
||||
# current. It can grasp an object without forcing too much even tho, it's goal position is a
|
||||
|
||||
@@ -91,34 +91,34 @@ class KochLeader(Teleoperator):
|
||||
def calibrate(self) -> None:
|
||||
logger.info(f"\nRunning calibration of {self}")
|
||||
self.arm.disable_torque()
|
||||
for name in self.arm.names:
|
||||
self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
|
||||
for motor in self.arm.motors:
|
||||
self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
|
||||
|
||||
self.arm.write("Drive_Mode", "elbow_flex", DriveMode.INVERTED.value)
|
||||
drive_modes = {name: 1 if name == "elbow_flex" else 0 for name in self.arm.names}
|
||||
drive_modes = {motor: 1 if motor == "elbow_flex" else 0 for motor in self.arm.motors}
|
||||
|
||||
input(f"Move {self} to the middle of its range of motion and press ENTER....")
|
||||
homing_offsets = self.arm.set_half_turn_homings()
|
||||
|
||||
full_turn_motors = ["shoulder_pan", "wrist_roll"]
|
||||
unknown_range_motors = [name for name in self.arm.names if name not in full_turn_motors]
|
||||
unknown_range_motors = [motor for motor in self.arm.motors if motor not in full_turn_motors]
|
||||
print(
|
||||
f"Move all joints except {full_turn_motors} sequentially through their "
|
||||
"entire ranges of motion.\nRecording positions. Press ENTER to stop..."
|
||||
)
|
||||
range_mins, range_maxes = self.arm.record_ranges_of_motion(unknown_range_motors)
|
||||
for name in full_turn_motors:
|
||||
range_mins[name] = 0
|
||||
range_maxes[name] = 4095
|
||||
for motor in full_turn_motors:
|
||||
range_mins[motor] = 0
|
||||
range_maxes[motor] = 4095
|
||||
|
||||
self.calibration = {}
|
||||
for name, motor in self.arm.motors.items():
|
||||
self.calibration[name] = MotorCalibration(
|
||||
id=motor.id,
|
||||
drive_mode=drive_modes[name],
|
||||
homing_offset=homing_offsets[name],
|
||||
range_min=range_mins[name],
|
||||
range_max=range_maxes[name],
|
||||
for motor, m in self.arm.motors.items():
|
||||
self.calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=drive_modes[motor],
|
||||
homing_offset=homing_offsets[motor],
|
||||
range_min=range_mins[motor],
|
||||
range_max=range_maxes[motor],
|
||||
)
|
||||
|
||||
self.arm.write_calibration(self.calibration)
|
||||
@@ -128,13 +128,13 @@ class KochLeader(Teleoperator):
|
||||
def configure(self) -> None:
|
||||
self.arm.disable_torque()
|
||||
self.arm.configure_motors()
|
||||
for name in self.arm.names:
|
||||
if name != "gripper":
|
||||
for motor in self.arm.motors:
|
||||
if motor != "gripper":
|
||||
# Use 'extended position mode' for all motors except gripper, because in joint mode the servos
|
||||
# can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while
|
||||
# assembling the arm, you could end up with a servo with a position 0 or 4095 at a crucial
|
||||
# point
|
||||
self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
|
||||
self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
|
||||
|
||||
# Use 'position control current based' for gripper to be limited by the limit of the current.
|
||||
# For the follower gripper, it means it can grasp an object without forcing too much even tho,
|
||||
|
||||
@@ -88,14 +88,14 @@ class SO100Leader(Teleoperator):
|
||||
def calibrate(self) -> None:
|
||||
logger.info(f"\nRunning calibration of {self}")
|
||||
self.arm.disable_torque()
|
||||
for name in self.arm.names:
|
||||
self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value)
|
||||
for motor in self.arm.motors:
|
||||
self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
|
||||
|
||||
input(f"Move {self} to the middle of its range of motion and press ENTER....")
|
||||
homing_offsets = self.arm.set_half_turn_homings()
|
||||
|
||||
full_turn_motor = "wrist_roll"
|
||||
unknown_range_motors = [name for name in self.arm.names if name != full_turn_motor]
|
||||
unknown_range_motors = [motor for motor in self.arm.motors if motor != full_turn_motor]
|
||||
print(
|
||||
f"Move all joints except '{full_turn_motor}' sequentially through their "
|
||||
"entire ranges of motion.\nRecording positions. Press ENTER to stop..."
|
||||
@@ -105,13 +105,13 @@ class SO100Leader(Teleoperator):
|
||||
range_maxes[full_turn_motor] = 4095
|
||||
|
||||
self.calibration = {}
|
||||
for name, motor in self.arm.motors.items():
|
||||
self.calibration[name] = MotorCalibration(
|
||||
id=motor.id,
|
||||
for motor, m in self.arm.motors.items():
|
||||
self.calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=0,
|
||||
homing_offset=homing_offsets[name],
|
||||
range_min=range_mins[name],
|
||||
range_max=range_maxes[name],
|
||||
homing_offset=homing_offsets[motor],
|
||||
range_min=range_mins[motor],
|
||||
range_max=range_maxes[motor],
|
||||
)
|
||||
|
||||
self.arm.write_calibration(self.calibration)
|
||||
@@ -121,8 +121,8 @@ class SO100Leader(Teleoperator):
|
||||
def configure(self) -> None:
|
||||
self.arm.disable_torque()
|
||||
self.arm.configure_motors()
|
||||
for name in self.arm.names:
|
||||
self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value)
|
||||
for motor in self.arm.motors:
|
||||
self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
|
||||
|
||||
def setup_motors(self) -> None:
|
||||
for motor in reversed(self.arm.motors):
|
||||
|
||||
@@ -88,34 +88,34 @@ class WidowX(Teleoperator):
|
||||
raise NotImplementedError # TODO(aliberts): adapt code below (copied from koch)
|
||||
logger.info(f"\nRunning calibration of {self}")
|
||||
self.arm.disable_torque()
|
||||
for name in self.arm.names:
|
||||
self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
|
||||
for motor in self.arm.motors:
|
||||
self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
|
||||
|
||||
self.arm.write("Drive_Mode", "elbow_flex", DriveMode.INVERTED.value)
|
||||
drive_modes = {name: 1 if name == "elbow_flex" else 0 for name in self.arm.names}
|
||||
drive_modes = {motor: 1 if motor == "elbow_flex" else 0 for motor in self.arm.motors}
|
||||
|
||||
input("Move robot to the middle of its range of motion and press ENTER....")
|
||||
homing_offsets = self.arm.set_half_turn_homings()
|
||||
|
||||
full_turn_motors = ["shoulder_pan", "wrist_roll"]
|
||||
unknown_range_motors = [name for name in self.arm.names if name not in full_turn_motors]
|
||||
unknown_range_motors = [motor for motor in self.arm.motors if motor not in full_turn_motors]
|
||||
print(
|
||||
f"Move all joints except {full_turn_motors} sequentially through their "
|
||||
"entire ranges of motion.\nRecording positions. Press ENTER to stop..."
|
||||
)
|
||||
range_mins, range_maxes = self.arm.record_ranges_of_motion(unknown_range_motors)
|
||||
for name in full_turn_motors:
|
||||
range_mins[name] = 0
|
||||
range_maxes[name] = 4095
|
||||
for motor in full_turn_motors:
|
||||
range_mins[motor] = 0
|
||||
range_maxes[motor] = 4095
|
||||
|
||||
self.calibration = {}
|
||||
for name, motor in self.arm.motors.items():
|
||||
self.calibration[name] = MotorCalibration(
|
||||
id=motor.id,
|
||||
drive_mode=drive_modes[name],
|
||||
homing_offset=homing_offsets[name],
|
||||
range_min=range_mins[name],
|
||||
range_max=range_maxes[name],
|
||||
for motor, m in self.arm.motors.items():
|
||||
self.calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=drive_modes[motor],
|
||||
homing_offset=homing_offsets[motor],
|
||||
range_min=range_mins[motor],
|
||||
range_max=range_maxes[motor],
|
||||
)
|
||||
|
||||
self.arm.write_calibration(self.calibration)
|
||||
|
||||
@@ -46,13 +46,13 @@ def dummy_calibration(dummy_motors) -> dict[str, MotorCalibration]:
|
||||
mins = [43, 27, 145]
|
||||
maxes = [1335, 3608, 3999]
|
||||
calibration = {}
|
||||
for name, motor in dummy_motors.items():
|
||||
calibration[name] = MotorCalibration(
|
||||
id=motor.id,
|
||||
drive_mode=drive_modes[motor.id - 1],
|
||||
homing_offset=homings[motor.id - 1],
|
||||
range_min=mins[motor.id - 1],
|
||||
range_max=maxes[motor.id - 1],
|
||||
for motor, m in dummy_motors.items():
|
||||
calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=drive_modes[m.id - 1],
|
||||
homing_offset=homings[m.id - 1],
|
||||
range_min=mins[m.id - 1],
|
||||
range_max=maxes[m.id - 1],
|
||||
)
|
||||
return calibration
|
||||
|
||||
|
||||
@@ -45,13 +45,13 @@ def dummy_calibration(dummy_motors) -> dict[str, MotorCalibration]:
|
||||
mins = [43, 27, 145]
|
||||
maxes = [1335, 3608, 3999]
|
||||
calibration = {}
|
||||
for name, motor in dummy_motors.items():
|
||||
calibration[name] = MotorCalibration(
|
||||
id=motor.id,
|
||||
for motor, m in dummy_motors.items():
|
||||
calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=0,
|
||||
homing_offset=homings[motor.id - 1],
|
||||
range_min=mins[motor.id - 1],
|
||||
range_max=maxes[motor.id - 1],
|
||||
homing_offset=homings[m.id - 1],
|
||||
range_min=mins[m.id - 1],
|
||||
range_max=maxes[m.id - 1],
|
||||
)
|
||||
return calibration
|
||||
|
||||
|
||||
Reference in New Issue
Block a user