Skip to content

Commit

Permalink
fixed brittle sensor issue, as per issue #62
Browse files Browse the repository at this point in the history
  • Loading branch information
@cremebrule
cremebrule committed Jun 12, 2020
1 parent ce86fe4 commit 33d23b7
Show file tree
Hide file tree
Showing 2 changed files with 55 additions and 28 deletions.
46 changes: 31 additions & 15 deletions robosuite/robots/bimanual.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,8 +81,8 @@ def __init__(
self._ref_joint_gripper_actuator_indexes = self._input2dict(None) # xml gripper (pos) actuator indexes for robot in mjsim
self.eef_site_id = self._input2dict(None) # xml element id for eef in mjsim
self.eef_cylinder_id = self._input2dict(None) # xml element id for eef cylinder in mjsim
self.eef_force_sensor_id = self._input2dict(None) # xml element id for eef force sensor in mjsim
self.eef_torque_sensor_id = self._input2dict(None) # xml element id for eef torque sensor in mjsim
self.eef_force_sensor_idx = self._input2dict(None) # start idx for eef force sensor in sensordata array
self.eef_torque_sensor_idx = self._input2dict(None) # start idx for eef torque sensor in sensordata array
self.torques = None # Current torques being applied

self.recent_qpos = None # Current and last robot arm qpos
Expand Down Expand Up @@ -245,9 +245,13 @@ def setup_references(self):
self.eef_cylinder_id[arm] = self.sim.model.site_name2id(
self.gripper[arm].visualization_sites["grip_cylinder"])

# IDs of eef sensors
self.eef_force_sensor_id[arm] = self.sim.model.sensor_name2id(self.gripper[arm].sensors["force_ee"])
self.eef_torque_sensor_id[arm] = self.sim.model.sensor_name2id(self.gripper[arm].sensors["torque_ee"])
# Indexes of eef sensors -- Note that this is the index of where the sensor data for this sensor starts
# in the sim.data.sensordata struct, NOT the ID assigned by mujoco!
sensordims = self.sim.model.sensor_dim
self.eef_force_sensor_idx[arm] = np.sum(
sensordims[:self.sim.model.sensor_name2id(self.gripper[arm].sensors["force_ee"])])
self.eef_torque_sensor_idx[arm] = np.sum(
sensordims[:self.sim.model.sensor_name2id(self.gripper[arm].sensors["torque_ee"])])

def control(self, action, policy_step=False):
"""
Expand Down Expand Up @@ -309,16 +313,7 @@ def control(self, action, policy_step=False):
self.recent_torques.push(self.torques)

for arm in self.arms:
self.recent_ee_forcetorques[arm].push(
np.concatenate(
(
self.sim.data.sensordata[self.eef_force_sensor_id[arm] * 3:
self.eef_force_sensor_id[arm] * 3 + 3],
self.sim.data.sensordata[self.eef_torque_sensor_id[arm] * 3:
self.eef_torque_sensor_id[arm] * 3 + 3]
)
)
)
self.recent_ee_forcetorques.push[arm](np.concatenate((self.ee_force[arm], self.ee_torque[arm])))
self.recent_ee_pose[arm].push(np.concatenate((self.controller[arm].ee_pos,
T.mat2quat(self.controller[arm].ee_ori_mat))))
self.recent_ee_vel[arm].push(np.concatenate((self.controller[arm].ee_pos_vel,
Expand Down Expand Up @@ -494,6 +489,27 @@ def ee_ft_integral(self):
vals[arm] = np.abs((1.0 / self.control_freq) * self.recent_ee_forcetorques[arm].average)
return vals

@property
def ee_force(self):
"""
Returns force applied at the force sensor at the robot arm's eef
"""
vals = {}
for arm in self.arms:
vals[arm] = self.sim.data.sensordata[self.eef_force_sensor_idx[arm]: self.eef_force_sensor_idx[arm] + 3]
return vals

@property
def ee_torque(self):
"""
Returns torque applied at the torque sensor at the robot arm's eef
"""
vals = {}
for arm in self.arms:
vals[arm] = self.sim.data.sensordata[self.eef_torque_sensor_idx[arm]: self.eef_torque_sensor_idx[arm] + 3]
return vals


@property
def _hand_pose(self):
"""
Expand Down
37 changes: 24 additions & 13 deletions robosuite/robots/single_arm.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,8 +75,8 @@ def __init__(
self._ref_joint_gripper_actuator_indexes = None # xml gripper (pos) actuator indexes for robot in mjsim
self.eef_site_id = None # xml element id for eef in mjsim
self.eef_cylinder_id = None # xml element id for eef cylinder in mjsim
self.eef_force_sensor_id = None # xml element id for eef force sensor in mjsim
self.eef_torque_sensor_id = None # xml element id for eef torque sensor in mjsim
self.eef_force_sensor_idx = None # start idx for eef force sensor in sensordata array
self.eef_torque_sensor_idx = None # start idx for eef torque sensor in sensordata array
self.torques = None # Current torques being applied

self.recent_qpos = None # Current and last robot arm qpos
Expand Down Expand Up @@ -217,9 +217,13 @@ def setup_references(self):
self.eef_site_id = self.sim.model.site_name2id(self.gripper.visualization_sites["grip_site"])
self.eef_cylinder_id = self.sim.model.site_name2id(self.gripper.visualization_sites["grip_cylinder"])

# IDs of eef sensors
self.eef_force_sensor_id = self.sim.model.sensor_name2id(self.gripper.sensors["force_ee"])
self.eef_torque_sensor_id = self.sim.model.sensor_name2id(self.gripper.sensors["torque_ee"])
# Indexes of eef sensors -- Note that this is the index of where the sensor data for this sensor starts in the
# sim.data.sensordata struct, NOT the ID assigned by mujoco!
sensordims = self.sim.model.sensor_dim
self.eef_force_sensor_idx = np.sum(
sensordims[:self.sim.model.sensor_name2id(self.gripper.sensors["force_ee"])])
self.eef_torque_sensor_idx = np.sum(
sensordims[:self.sim.model.sensor_name2id(self.gripper.sensors["torque_ee"])])

def control(self, action, policy_step=False):
"""
Expand Down Expand Up @@ -270,14 +274,7 @@ def control(self, action, policy_step=False):
self.recent_qpos.push(self._joint_positions)
self.recent_actions.push(action)
self.recent_torques.push(self.torques)
self.recent_ee_forcetorques.push(
np.concatenate(
(
self.sim.data.sensordata[self.eef_force_sensor_id * 3 : self.eef_force_sensor_id * 3 + 3],
self.sim.data.sensordata[self.eef_torque_sensor_id * 3: self.eef_torque_sensor_id * 3 + 3]
)
)
)
self.recent_ee_forcetorques.push(np.concatenate((self.ee_force, self.ee_torque)))
self.recent_ee_pose.push(np.concatenate((self.controller.ee_pos, T.mat2quat(self.controller.ee_ori_mat))))
self.recent_ee_vel.push(np.concatenate((self.controller.ee_pos_vel, self.controller.ee_ori_vel)))

Expand Down Expand Up @@ -416,6 +413,20 @@ def ee_ft_integral(self):
"""
return np.abs((1.0 / self.control_freq) * self.recent_ee_forcetorques.average)

@property
def ee_force(self):
"""
Returns force applied at the force sensor at the robot arm's eef
"""
return self.sim.data.sensordata[self.eef_force_sensor_idx: self.eef_force_sensor_idx + 3]

@property
def ee_torque(self):
"""
Returns torque applied at the torque sensor at the robot arm's eef
"""
return self.sim.data.sensordata[self.eef_torque_sensor_idx: self.eef_torque_sensor_idx + 3]

@property
def _right_hand_pose(self):
"""
Expand Down

0 comments on commit 33d23b7

Please sign in to comment.