Issues about writing a joint impedance controller for a panda robot

[Yingfan99327]

Hi,

I'm a student and I'm trying to use MuJoCo for robotic research.

I'm looking for some help with how to enable the robot to track the desired joint trajectories accurately.

Following this issue https://github.com/google-deepmind/mujoco/issues/954, which is close due to lack of activity. What I am trying to do is very similar that I am trying to write a joint impedance controller for a panda robot in mujoco. The desired positions I directly send to each joint is defined as follows,

desired_qpos[i] = np.sin((self.data.time) * np.pi)
desired_qvel[i] = np.cos((self.data.time) * np.pi)
desired_qacc[i] = -np.sin((self.data.time) * np.pi)

that I hope the torque ouput computed from the impedance controller could enable the each joint of the robot to accurately follow the sin trajectory with an error fewer than 1~2 degree. Here, I also consider the joint limits of each joint (because I hope to make the simulation closer to reality as much as possible) and adjust the desired sin trajectories correspondingly so that each joint will not exceed the joint limits during motion. For instance, the joint limit for the 4-th joint is [-0.0698, -3.0718], so I adjusted the 4-th joint's desired position input as,

self.desired_qpos[3] = 0.05 * np.sin((self.data.time) * np.pi) - 0.12    
self.desired_qvel[3] = 0.05 * np.cos((self.data.time) * np.pi)
self.desired_qacc[3] = -0.05 * np.sin((self.data.time) * np.pi)

, the same as the other joints.

However, no matter how I adjusted the joint impedance parameters, the step size, the intergrator, I still cannot get the expected accuracy I hope as the figure below illustrates, which I set the timestep = 0.001s to match the robot control frequency in real scene. I just picked the error trajectoris of the first two joints and the unit for the y-axis is radian.

Based on the problem I explained above, I'd like to ask several questions as follows, 1) I tried two different approaches to setting the timestep and check whether I modified the timestep successfully in simulation by printing the self.data.time after calling the mj_step function each time. The first approach is to modifying the xml file by setting that,

<option integrator="implicitfast" impratio="10" timestep = "0.001" />

, however, it doesn't work I think. While the second approach is to set

self.model.opt.timestep = 0.001

which works, so may I ask why the first approach doesn't work, did I miss something? Is it correct to change the timestep by using the second approach?

2. Suppose the controller codes part is correct, what could result in the not good tracking performance? I tried all the integrators, however, they all present the identical or similar error results. I am curious that if I write the controller in a right way, select proper controller parameters, timesteps (0.001 I guess should be enough) and setting the joint limits, the robot should demonstrate perfect tracking performance in the simulator, right?
3. Will the "impratio" parameter influence the results, what would be its recommended value for simulation?
4. How to ensure the motion demonstrated from the Mujoco simulator close to the real experiment, would there be some important parameters I miss to set or define in the model xml files?

Thanks very much for your time and wishing you all the best.

[Balint-H]

Hello! Why do you suspect the timestep is set incorrectly with the XML? But yes, you can update the timestep in your model via script.

I recommend setting your control via control callback and visualize your robots movement in the simulate window (here's an example). Could help identify if e.g. you are colliding with something in your scene, or play around with parameters/perturbations. You could also increase the controller's parameters to achieve stricter tracking.

Also consider if the robot has any passive damping resisting the movement.

If you do want to decrease the timestep but maintain the control frequency, you can perform the control on every Nth time step, and skip it otherwise.

Lastly, the controller implementation will have an effect on tracking lag of course. E.g., if you only consider dof's individually when controlling them, you miss the coupled contributions from other joints affecting the hierarchy. I suspect that this is the cause of q2's peak in error, as that is close to the time of the peak acceleration in q1.

[Yingfan99327]

Thanks so much for your suggestions, I will try them in my controller design. Really appreciate and wishing you all the best.

[Yingfan99327]

Hi, @Balint-H, Thanks so much for your reply and your help.

Do you have any comments on this issue #1909 I just proposed.