Additional sensors

[TanKarabudakk]

Hello, what are the steps I should take if I want to collect data from additional sensors lets say torque sensors?

[Danfoa]

Hi @TanKarabudakk

The idea of the QuadrupedEnv class is to provide an clean and hackable abstraction of a quadruped robot dynamics using gym API.

The environment has a list of observations you can request to be in the "observation space" of the environment. If the observation you desire is not in the available list:

gym-quadruped/gym_quadruped/quadruped_env.py

Lines 35 to 81 in fe29c07

	BASE_OBS = [
	'base_pos',
	'base_lin_vel',
	'base_lin_vel_err',
	'base_lin_acc',
	'base_ang_vel',
	'base_ang_vel_err',
	'base_ori_euler_xyz',
	'base_ori_quat_wxyz',
	'base_ori_SO3',
	'gravity_vector:base',
	]
	BASE_OBS_BASE_FRAME = [
	'base_lin_vel:base',
	'base_lin_vel_err:base',
	'base_lin_acc:base',
	'base_ang_vel:base',
	'base_ang_vel_err:base',
	]
	GEN_COORDS_OBS = ['qpos', 'qvel', 'tau_ctrl_setpoint', 'qpos_js', 'qvel_js', 'kinetic_energy', 'work']
	FEET_OBS = [
	'feet_pos',
	'feet_pos:base',
	'feet_vel',
	'feet_vel_rel',
	'feet_vel:base',
	'feet_vel_rel:base',
	'contact_state',
	'contact_forces',
	'contact_forces:base',
	]
	VelCallable = Callable[[float], np.ndarray] # time[s] -> vec velocity [m/s] (3,)
	class QuadrupedEnv(gym.Env):
	"""A simple quadruped environment for testing model-based controllers and imitation learning algorithms.
	To deal with different quadruped robots, which might have different joint naming and ordering conventions, this
	environment uses the `LegsAttr` dataclass to store attributes associated with the legs of a quadruped robot. This
	dataclass uses the naming convention FL, FR, RL, RR to represent the Front-Left, Front-Right, Rear-Right, and
	Rear-Left legs, respectively.
	"""
	_DEFAULT_OBS = ('qpos', 'qvel', 'tau_ctrl_setpoint', 'feet_pos:base', 'feet_vel:base')
	ALL_OBS = BASE_OBS + BASE_OBS_BASE_FRAME + GEN_COORDS_OBS + FEET_OBS

Note that perhaps the observation you are looking for is: tau_ctrl_setpoint

Then you should

1. Add it to this list. For instance for torque readings you add it to the GEN_COORDS_OBS list.
2. Define the dimension of the observation here:

   gym-quadruped/gym_quadruped/utils/quadruped_utils.py

   Lines 235 to 325 in fe29c07

   	def configure_observation_space(mj_model: mujoco.MjModel, obs_names: Sequence[str]) -> [spaces.Dict]:
   	"""Configures the observation space for the environment based on the provided state observation names.
   	Args:
   	----
   	mj_model (mujoco.MjModel): The Mujoco model of the robot.
   	obs_names (list[str]): A list of state observation names based on which the observation space is
   	configured.
   	Returns:
   	-------
   	gym.spaces.Dict: A dictionary with a gym.spaces.Box for each observation name.
   	"""
   	obs_spaces = {}
   	qpos_lim_min, qpos_lim_max = mj_model.jnt_range[:, 0], mj_model.jnt_range[:, 1]
   	tau_lim_min, tau_lim_max = mj_model.actuator_ctrlrange[:, 0], mj_model.actuator_ctrlrange[:, 1]
   	for obs_name in obs_names:
   	if obs_name == 'qpos':
   	obs_dim = mj_model.nq
   	obs_lim_max = [np.inf] * 7 + qpos_lim_max[1:].tolist() # Ignore the base position
   	obs_lim_min = [-np.inf] * 7 + qpos_lim_min[1:].tolist() # Ignore the base position
   	elif obs_name == 'qvel':
   	obs_dim = mj_model.nv
   	obs_lim_max = [np.inf] * obs_dim
   	obs_lim_min = [-np.inf] * obs_dim
   	elif obs_name == 'tau_ctrl_setpoint':
   	obs_dim = mj_model.nu
   	obs_lim_max = tau_lim_max
   	obs_lim_min = tau_lim_min
   	elif obs_name == 'qpos_js':
   	obs_dim = mj_model.nq - 7
   	obs_lim_max = qpos_lim_max[1:]
   	obs_lim_min = qpos_lim_min[1:]
   	elif obs_name == 'qvel_js':
   	obs_dim = mj_model.nv - 6
   	obs_lim_max = [np.inf] * obs_dim
   	obs_lim_min = [-np.inf] * obs_dim
   	elif (
   	obs_name == 'base_pos'
   	or 'base_lin_vel' in obs_name
   	or 'base_lin_acc' in obs_name
   	or 'base_ang_vel' in obs_name
   	or 'base_ori_euler_xyz' in obs_name
   	):
   	obs_dim = 3
   	obs_lim_max = [np.inf] * obs_dim
   	obs_lim_min = [-np.inf] * obs_dim
   	elif obs_name == 'base_ori_quat_wxyz':
   	obs_dim = 4
   	obs_lim_max = [np.inf] * obs_dim
   	obs_lim_min = [-np.inf] * obs_dim
   	elif obs_name == 'base_ori_SO3':
   	obs_dim = 9
   	obs_lim_max = [np.inf] * obs_dim
   	obs_lim_min = [-np.inf] * obs_dim
   	elif 'feet_pos' in obs_name or 'feet_vel' in obs_name:
   	obs_dim = 12
   	obs_lim_max = [np.inf] * obs_dim
   	obs_lim_min = [-np.inf] * obs_dim
   	elif obs_name == 'contact_state':
   	obs_dim = 4
   	obs_lim_max = [1] * obs_dim
   	obs_lim_min = [0] * obs_dim
   	elif 'contact_forces' in obs_name:
   	obs_dim = 12
   	obs_lim_max = [np.inf] * obs_dim
   	obs_lim_min = [-np.inf] * obs_dim
   	elif 'gravity_vector' in obs_name or 'imu' in obs_name:
   	obs_dim = 3
   	obs_lim_max = [np.inf] * obs_dim
   	obs_lim_min = [-np.inf] * obs_dim
   	elif obs_name in ['work', 'kinetic_energy']:
   	obs_dim = 1
   	obs_lim_max = [np.inf]
   	obs_lim_min = [-np.inf]
   	else:
   	from gym_quadruped.quadruped_env import QuadrupedEnv
   	raise ValueError(f'Invalid observation name: {obs_name}, available obs: {QuadrupedEnv.ALL_OBS}')
   	obs_spaces[obs_name] = spaces.Box(
   	shape=(obs_dim,),
   	low=np.array(obs_lim_min),
   	high=np.array(obs_lim_max),
   	dtype=np.float32,
   	)
   	observation_space = spaces.Dict(obs_spaces)
   	return observation_space

3. And lastly, you should add the correct mujoco code logic to compute the observation that you want here:

   gym-quadruped/gym_quadruped/quadruped_env.py

   Lines 1142 to 1222 in fe29c07

   	def _get_obs(self):
   	"""Returns the state observation based on the specified state observation names."""
   	state_obs_dict = {}
   	remaining_obs = set(self.observation_space.keys())
   	for obs_name in self.state_obs_names:
   	frame = 'world' if not obs_name.endswith('base') else 'base'
   	obs_val = None # Initialize observation value
   	# Generalized position, velocity, and force (torque) spaces
   	if obs_name == 'qpos':
   	obs_val = self.mjData.qpos.copy()
   	elif obs_name == 'qvel':
   	obs_val = self.mjData.qvel.copy()
   	elif obs_name == 'tau_ctrl_setpoint':
   	obs_val = self.torque_ctrl_setpoint.copy()
   	elif obs_name == 'qpos_js':
   	obs_val = self.mjData.qpos[7:].copy()
   	elif obs_name == 'qvel_js':
   	obs_val = self.mjData.qvel[6:].copy()
   	elif obs_name == 'base_pos':
   	obs_val = self.base_pos.copy()
   	elif 'base_lin_vel_err' in obs_name:
   	obs_val = self.base_lin_vel_err(frame).copy()
   	elif 'base_lin_vel' in obs_name:
   	obs_val = self.base_lin_vel(frame).copy()
   	elif 'base_lin_acc' in obs_name:
   	obs_val = self.base_lin_acc(frame).copy()
   	elif 'base_ang_vel_err' in obs_name:
   	obs_val = self.base_ang_vel_err(frame).copy()
   	elif 'base_ang_vel' in obs_name:
   	obs_val = self.base_ang_vel(frame).copy()
   	elif obs_name == 'base_ori_euler_xyz':
   	obs_val = self.base_ori_euler_xyz.copy()
   	elif obs_name == 'base_ori_quat_wxyz':
   	obs_val = self.mjData.qpos[3:7].copy()
   	elif obs_name == 'base_ori_SO3':
   	obs_val = self.base_configuration[0:3, 0:3].flatten().copy()
   	elif 'feet_pos' in obs_name:
   	obs_val = np.concatenate(self.feet_pos(frame).to_list(order=self.legs_order), axis=0).copy()
   	elif 'feet_vel_rel' in obs_name:
   	obs_val = np.concatenate(
   	self.feet_vel(frame, relative=True).to_list(order=self.legs_order), axis=0
   	).copy()
   	elif 'feet_vel' in obs_name:
   	obs_val = np.concatenate(
   	self.feet_vel(frame, relative=False).to_list(order=self.legs_order), axis=0
   	).copy()
   	elif obs_name == 'contact_state':
   	contact_state, _ = self.feet_contact_state()
   	obs_val = np.array(contact_state.to_list(), dtype=np.float32).copy()
   	elif 'contact_forces' in obs_name:
   	_, _, contact_forces = self.feet_contact_state(ground_reaction_forces=True, frame=frame)
   	obs_val = np.concatenate(contact_forces.to_list(order=self.legs_order), axis=0).copy()
   	elif obs_name == 'gravity_vector:base':
   	obs_val = self.gravity_vector.copy()
   	elif obs_name == 'work':
   	obs_val = np.atleast_1d(self.work)
   	elif obs_name == 'kinetic_energy':
   	obs_val = np.atleast_1d(self.kinetic_energy)
   	else: # If not a predefined observation, check if it is a sensor observation
   	is_sensor_obs = False
   	for sensor in self.sensors:
   	if obs_name in sensor.available_observations():
   	obs_val = sensor.get_observation(obs_name)
   	is_sensor_obs = True
   	break
   	if not is_sensor_obs:
   	raise ValueError(f'Invalid observation name: {obs_name}, available obs: {self.ALL_OBS}')
   	assert self.observation_space[obs_name].shape == obs_val.shape, (
   	f'Invalid shape for observation {obs_name}: {obs_val.shape} != {self.observation_space[obs_name].shape}'
   	)
   	state_obs_dict[obs_name] = obs_val # Assign computed observation value to the dictionary
   	remaining_obs.remove(obs_name) # Remove the observation name from the remaining observations
   	if len(remaining_obs) > 0:
   	raise RuntimeError(
   	f'The observations {remaining_obs} are in the observation space but not in the returned obsertation.'
   	)
   	return state_obs_dict

Note that mujoco does support adding mujoco torque sensors to each DoF of the robot. In general, not all robot description files do have these sensors defined in the .xml, so you can do two things:

1. fast and dirty Simply use perfect sensor readings (no noise) from mujoco data, as shown here (for the legs DoF):

   gym-quadruped/gym_quadruped/quadruped_env.py

   Lines 897 to 918 in fe29c07

   	def legs_qfrc_bias(self):
   	"""Gets the Coriolis and centrifugal forces acting on the legs."""
   	# centrifugal, coriolis, gravity
   	legs_qfrc_bias = LegsAttr(
   	FL=self.mjData.qfrc_bias[self.legs_qvel_idx.FL],
   	FR=self.mjData.qfrc_bias[self.legs_qvel_idx.FR],
   	RL=self.mjData.qfrc_bias[self.legs_qvel_idx.RL],
   	RR=self.mjData.qfrc_bias[self.legs_qvel_idx.RR],
   	)
   	return legs_qfrc_bias
   	@property
   	def legs_qfrc_passive(self):
   	"""Gets the passive forces acting on the legs, e.g. friction."""
   	# passive forces
   	legs_qfrc_passive = LegsAttr(
   	FL=self.mjData.qfrc_passive[self.legs_qvel_idx.FL],
   	FR=self.mjData.qfrc_passive[self.legs_qvel_idx.FR],
   	RL=self.mjData.qfrc_passive[self.legs_qvel_idx.RL],
   	RR=self.mjData.qfrc_passive[self.legs_qvel_idx.RR],
   	)
   	return legs_qfrc_passive

2. Ideally we would want to programatically add the torque sensors at each joint for all robots, with parametric noise that can be configured by the user at the initialization of the env. This would require some work, not difficult, but its the longer path that would lead to a contribution that we would appreciate.