osc.py & input_type='absolute' & impedance_mode

[LucienJi]

System Info

Ubuntu, master branch

Information

For the controller config, if the input_type = "absolute", and impedance_type is not fixed, the self.goal_pos should be delta[0:3] instead of action[0:3], self.goal_ori has the similar issue.

Reproduction

`

Parse action based on the impedance mode, and update kp / kd as necessary

    if self.impedance_mode == "variable":
        damping_ratio, kp, delta = action[:6], action[6:12], action[12:]
        self.kp = np.clip(kp, self.kp_min, self.kp_max)
        self.kd = 2 * np.sqrt(self.kp) * np.clip(damping_ratio, self.damping_ratio_min, self.damping_ratio_max)
    elif self.impedance_mode == "variable_kp":
        kp, delta = action[:6], action[6:]
        self.kp = np.clip(kp, self.kp_min, self.kp_max)
        self.kd = 2 * np.sqrt(self.kp)  # critically damped
    else:  # This is case "fixed"
        delta = action

    # If we're using deltas, interpret actions as such
    if self.input_type == "delta":
        scaled_delta = self.scale_action(delta)
        self.goal_pos = self.compute_goal_pos(scaled_delta[0:3])
        if self.use_ori is True:
            self.goal_ori = self.compute_goal_ori(scaled_delta[3:6])
        else:
            self.goal_ori = self.compute_goal_ori(np.zeros(3))
    # Else, interpret actions as absolute values
    elif self.input_type == "absolute":
        self.goal_pos = action[0:3]
        if self.use_ori is True:
            self.goal_ori = Rotation.from_rotvec(action[3:6]).as_matrix()
        else:
            self.goal_ori = self.compute_goal_ori(np.zeros(3))
    else:
        raise ValueError(f"Unsupport input_type {self.input_type}")

`

Expected behavior

No response