OSC Position / Pose Controller has steady-state error

[ismarou]

Hello,

As I am trying to implement a project using the Robosuite environments, I observed that the OSC Controllers with the default gains provided cannot track desired poses (not even just 1D positions) at 100% accuracy. In more details, when I call env.step(action), where action = [where I want to be at the next step] - [where I am at this step], I observed an error of about 0.6 at average.

Is this desirable and/or known to you? Am I missing something or doing something wrong?

[zhuyifengzju]

Hi, OSC controller is not expected to precisely track a trajectory by just doing env.step(action). If you want to track a trajectory of desired poses, you would need to write a program/function on top of the OSC controller that has some while-loop logic. In this while loop, you need to ensure that the arm hits every desired pose before setting the next desired pose.

[hai-h-nguyen]

Hi, OSC controller is not expected to precisely track a trajectory by just doing env.step(action). If you want to track a trajectory of desired poses, you would need to write a program/function on top of the OSC controller that has some while-loop logic. In this while loop, you need to ensure that the arm hits every desired pose before setting the next desired pose.

Would it really be the case or that is the issue of the PID parameters are not good enough? I took a look at the OSC PID controllers and it seems there is no integrator part.

[roberto-martinmartin]

OSC controller is only doing PD control. But in general, if you want to track a trajectory, the best would be to do some form of motion generation. For example, find the path you want to follow and generate desired positions, velocities and accelerations to follow the path (e.g., with some high-level spline). Then, you would need to implement a trajectory controller in robosuite that accepts not only positions but also derivatives.

The alternative, if there are no requirements in the full path, just passing through some points, is to do as Yifeng mentioned: add some additional code on top of the controller to verify convergence and switch to the next via point.

Increasing Kp and Kv would alleviate the issue but will increase instabilities. And a controller will always have some tracking error when use to follow a changing goal.

If you let the controller run for several timesteps it should converge to the desired position, you don't need an integral term because there shouldn't be any large deviation in the dynamics. I'm assuming the problem is that the controller is used to "track" a changing goal, not to converge to a constant one. If the controller is not converging to a constant goal after several steps, please, let us know.

Thanks!

[omkarpatil18]

EDIT: I was using Euler angles instead of axis angles :/ . It works now. Editing the script here in case anyone wants to use it for OSC control. (the discretization and decay can be handled in a much better manner than here). Lmk if you find any more errors. Thanks!

def osc_control_to(self, ref_state, desc=20, threshold=0.1):
        """
        Works for most robots
        Args
        ----
        ref_state: np.array([x, y, z, ax, ay, az, grip])
        desc: Number of waypoints to goal
        threshold: Error allowed

        Notes
        -----
        Control using OSC
        Might need tuning for each robot based on the impedances
        """
        env = self.env
        robot = env.robots[0]
        controller = robot.controller

        # control logic
        ref_pose = ref_state[:-1]
        gripper_pos = ref_state[-1]
        cur_pose = np.array(
            [*controller.ee_pos, *T.quat2axisangle(T.mat2quat(controller.ee_ori_mat))]
        )
        ori_delta = (ref_pose - cur_pose) / desc
        next_pose = cur_pose + ori_delta
        step_itr = 1
        while np.linalg.norm(ref_pose - cur_pose) > threshold:
            itr = 0
            delta = ori_delta
            while np.linalg.norm(next_pose - cur_pose) > threshold:
                env.step(np.concatenate((delta, np.array([gripper_pos])), 0))
                env.render()
                cur_pose = np.array(
                    [
                        *controller.ee_pos,
                        *T.quat2axisangle(T.mat2quat(controller.ee_ori_mat)),
                    ]
                )
                delta = next_pose - cur_pose
                # to avoid oscillations
                # ideally should decay kp?
                delta = delta / np.log2(2 + itr)  # slowly decaying
                itr += 1
                if itr > 200:
                    break
            if step_itr < desc:
                next_pose += ori_delta
                step_itr += 1
        pass

[Felixvillas]

EDIT: I was using Euler angles instead of axis angles :/ . It works now. Editing the script here in case anyone wants to use it for OSC control. (the discretization and decay can be handled in a much better manner than here). Lmk if you find any more errors. Thanks!

def osc_control_to(self, ref_state, desc=20, threshold=0.1):
        """
        Works for most robots
        Args
        ----
        ref_state: np.array([x, y, z, ax, ay, az, grip])
        desc: Number of waypoints to goal
        threshold: Error allowed

        Notes
        -----
        Control using OSC
        Might need tuning for each robot based on the impedances
        """
        env = self.env
        robot = env.robots[0]
        controller = robot.controller

        # control logic
        ref_pose = ref_state[:-1]
        gripper_pos = ref_state[-1]
        cur_pose = np.array(
            [*controller.ee_pos, *T.quat2axisangle(T.mat2quat(controller.ee_ori_mat))]
        )
        ori_delta = (ref_pose - cur_pose) / desc
        next_pose = cur_pose + ori_delta
        step_itr = 1
        while np.linalg.norm(ref_pose - cur_pose) > threshold:
            itr = 0
            delta = ori_delta
            while np.linalg.norm(next_pose - cur_pose) > threshold:
                env.step(np.concatenate((delta, np.array([gripper_pos])), 0))
                env.render()
                cur_pose = np.array(
                    [
                        *controller.ee_pos,
                        *T.quat2axisangle(T.mat2quat(controller.ee_ori_mat)),
                    ]
                )
                delta = next_pose - cur_pose
                # to avoid oscillations
                # ideally should decay kp?
                delta = delta / np.log2(2 + itr)  # slowly decaying
                itr += 1
                if itr > 200:
                    break
            if step_itr < desc:
                next_pose += ori_delta
                step_itr += 1
        pass

Hi~ @omkarpatil18 Excuse me, is this "osc_control_to" you implemented for the environment configuration with "control_config=True"?

[roberto-martinmartin]

I'm not sure what the goal was here because I do not see the old message, but if you want to interpolate orientations, I'd recommend using SLERP and quaternions for the orientation. Interpolating with quaternions is one of the main advantages of using them as representation for orientation. Any other representation of orientations (e.g., axis angle, Euler angles, rotation matrix) could lead to strange behaviors when interpolating and is not ensured to provide a proper geodesic.

[omkarpatil18]

control_config

Sorry I don't understand your question @Felixvillas. Could you please elaborate?

[Felixvillas]

control_config

Sorry I don't understand your question @Felixvillas. Could you please elaborate?

Thanks~ My doubts should have been cleared up.

In the OSC_POSE controller, there is a parameter called control_delta, which determines whether the input action is a relative command or an absolute command. I tried your code, and it seems that the action here is a relative command, corresponding to the parameter being control_delta=True.