Confusion about linear_interpolator and control frequency

[zichunxx]

Hi Robosuite team!

When I checked the linear_interpolator, I was confused about some code snippets. The following examples are all based on the osc_position controller. Please correct me if I miss something. Thanks in advance for checking this issue!

1. The update of the interpolation start point:

robosuite/robosuite/environments/base.py

Lines 387 to 397 in 48c1b8a

	policy_step = True
	# Loop through the simulation at the model timestep rate until we're ready to take the next policy step
	# (as defined by the control frequency specified at the environment level)
	for i in range(int(self.control_timestep / self.model_timestep)):
	self.sim.forward()
	self._pre_action(action, policy_step)
	self.sim.step()
	self._update_observables()
	policy_step = False

The interpolation goal point is updated only after receiving a new policy signal, i.e., policy_step is True. However, when set_goal is called for the first time, the start point is set equal to self.goal, which is a zero numpy array, even the current position of the end-effector is not in the original point.

robosuite/robosuite/controllers/interpolators/linear_interpolator.py

Lines 96 to 97 in 48c1b8a

	self.start = np.array(self.goal)
	self.goal = np.array(goal)

Then, on each acquisition of the interpolation target with get_interpolated_goal, x is assigned through self.start and is equal to zero. Also, self.start will not be updated until policy_step is reset to True.

robosuite/robosuite/controllers/interpolators/linear_interpolator.py

Lines 112 to 130 in 48c1b8a

	x = np.array(self.start)
	# Calculate the desired next step based on remaining interpolation steps
	if self.ori_interpolate is not None:
	# This is an orientation interpolation, so we interpolate linearly around a sphere instead
	goal = np.array(self.goal)
	if self.ori_interpolate == "euler":
	# this is assumed to be euler angles (x,y,z), so we need to first map to quat
	x = T.mat2quat(T.euler2mat(x))
	goal = T.mat2quat(T.euler2mat(self.goal))
	# Interpolate to the next sequence
	x_current = T.quat_slerp(x, goal, fraction=(self.step + 1) / self.total_steps)
	if self.ori_interpolate == "euler":
	# Map back to euler
	x_current = T.mat2euler(T.quat2mat(x_current))
	else:
	# This is a normal interpolation
	dx = (self.goal - x) / (self.total_steps - self.step)
	x_current = x + dx

The above did not affect the arm movement significantly since the step size is small. However, I think this can lead to a miscalculation of the position error and thus a deviation in the torque calculation. Is that right?

2. control_freq and policy_freq:

control_freq and policy_freq define how the policy is executed. However, I found them being misused on some occasions, e.g.,

robosuite/robosuite/robots/single_arm.py

Line 131 in 48c1b8a

self.controller_config["policy_freq"] = self.control_freq

robosuite/robosuite/controllers/controller_factory.py

Lines 113 to 120 in 48c1b8a

	interpolator = None
	if params["interpolation"] == "linear":
	interpolator = LinearInterpolator(
	ndim=params["ndim"],
	controller_freq=(1 / params["sim"].model.opt.timestep),
	policy_freq=params["policy_freq"],
	ramp_ratio=params["ramp_ratio"],
	)

3. How to adjust the control policy frequency

According to the code and document, interpolation steps can be adjusted by ramp_ratio. Is this related to the adjustment of the control frequency? I found the controller is called only once in this loop.

robosuite/robosuite/environments/base.py

Lines 387 to 397 in 48c1b8a

	policy_step = True
	# Loop through the simulation at the model timestep rate until we're ready to take the next policy step
	# (as defined by the control frequency specified at the environment level)
	for i in range(int(self.control_timestep / self.model_timestep)):
	self.sim.forward()
	self._pre_action(action, policy_step)
	self.sim.step()
	self._update_observables()
	policy_step = False