Skip to content

BUGFIX: Making indexing agnostic to of Robot Config order #168

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

Merged
vikashplus merged 1 commit into from
Jul 3, 2026
+27 −22
Merged

BUGFIX: Making indexing agnostic to of Robot Config order #168

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
49 changes: 27 additions & 22 deletions robohive/robot/robot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,10 @@
# Support for non uniform noise in sensor readings
# Support for noisy actions + separate noise_scale for sensor and actuator
# rename pos/vel to act/delta_act
# Improve space definitions
# sim_id: ID of the sensor/actuator in the sim
# act_id: ID of the sensor/actuator in the robot_config (hardware) space ==> Rename to hdr_id (robot_config unified different hardware into a single unified hardware space)
# hdr_id: ID of the sensor/actuator in the hardware space (e.g. dynamixel) ==> Rename to adr (This is the address that is used to communicate with the hardware. It is not necessarily the same as the act_id or sim_id.)

# NOTE/ GOOD PRACTICES ===========================
# nq should be nv
Expand Down Expand Up @@ -229,7 +233,14 @@ def hardware_get_sensors(self):


# apply controls to hardware
def hardware_apply_controls(self, control, is_reset=False):
def hardware_apply_controls(self, control, space='hdr', is_reset=False):
"""
control: control vector in hdr or sim space
space: 'hdr' or 'sim' (defaults to 'hdr' as represented in robot_config)
is_reset: if True, reset the hardware to the control values
"""

act_id = -1 # IDs as per the config order
for name, device in self.robot_config.items():
if 'actuator' in device.keys() and len(device['actuator'])>0:
if device['interface']['type'] == 'dynamixel':
Expand All @@ -239,8 +250,10 @@ def hardware_apply_controls(self, control, is_reset=False):
pwm_ctrl = []
pwm_ids = []
for actuator in device['actuator']:
act_id += 1
ctrl = control[actuator['sim_id']] if space == 'sim' else control[act_id]
# calibrate
calib_ctrl = control[actuator['sim_id']]*actuator['scale']+ actuator['offset']
calib_ctrl = ctrl*actuator['scale']+ actuator['offset']
if actuator['mode'] == 'Position':
pos_ids.append(actuator['hdr_id'])
pos_ctrl.append(calib_ctrl)
Expand All @@ -249,34 +262,26 @@ def hardware_apply_controls(self, control, is_reset=False):
pwm_ctrl.append(calib_ctrl)
else:
print("ERROR: Mode not found")
raise NotImplemented
raise NotImplementedError(f"ERROR: Actuator mode {actuator['mode']} not found")
# send controls
if pos_ids:
device['robot'].set_des_pos(pos_ids, pos_ctrl)
if pwm_ids:
device['robot'].set_des_pwm(pwm_ids, pwm_ctrl)

elif device['interface']['type'] == 'franka':
franka_des_pos = []
for actuator in device['actuator']:
# calibrate
franka_des_pos.append(control[actuator['sim_id']]*actuator['scale']+ actuator['offset'])
if is_reset:
device['robot'].reset(franka_des_pos)
else:
device['robot'].apply_commands(franka_des_pos)

elif device['interface']['type'] == 'robotiq':
robotiq_des_pos = []
elif device['interface']['type'] in ['franka', 'robotiq']:
des_pos = []
for actuator in device['actuator']:
act_id += 1
ctrl = control[actuator['sim_id']] if space == 'sim' else control[act_id]
# calibrate
robotiq_des_pos.append(control[actuator['sim_id']]*actuator['scale']+ actuator['offset'])
des_pos.append(ctrl*actuator['scale']+ actuator['offset'])
if is_reset:
device['robot'].reset(robotiq_des_pos[0])
device['robot'].reset(des_pos)
else:
device['robot'].apply_commands(robotiq_des_pos[0])
device['robot'].apply_commands(des_pos)
else:
raise NotImplemented("ERROR: interface not found")
raise NotImplementedError("ERROR: interface not found")


# close hardware
Expand Down Expand Up @@ -561,7 +566,7 @@ def normalize_actions(self, controls, out_space='sim', unnormalize=False):
Recover actions from unit space to absolute space; if unnormalize==True
in_space for controls has to be 'sim'
"""
act_id = -1
act_id = -1 # IDs as per the config order
controls_out = controls.copy()
for name, device in self.robot_config.items():
if name == "default_robot":
Expand Down Expand Up @@ -614,7 +619,7 @@ def process_actuator(self, controls, step_duration,
"""
# last_obs = self.get_sensor_from_cache(-1)
processed_controls = controls.copy()
act_id = -1
act_id = -1 # IDs as per the config order
for name, device in self.robot_config.items():
if name == "default_robot":
if self._act_mode == "pos":
Expand Down Expand Up @@ -685,7 +690,7 @@ def step(self, ctrl_desired, step_duration, ctrl_normalized=True, realTimeSim=Fa

# Send controls to the robot
if self.is_hardware:
self.hardware_apply_controls(ctrl_feasible)
self.hardware_apply_controls(ctrl_feasible, space=robot_type)
if render_cbk:
render_cbk()
else:
Expand Down
Loading