Closed ismarou closed 4 days ago
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.
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.
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!
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
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?