How to plan the robot to stop perfectly after tracking a reference trajectory or moving for some time with external force applied in mujoco?

Hi,

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

I'm looking for some help with motion planning task in mujoco with joint torque control signals.

What I aim to do is to write a admittance controller for a franka panda robot in mujoco and basically to fulfill two different catergories of tasks:

1) Follow a defined trajectory in cartesian space 2) Move when some external force is applied to the end-effector and stop quickly and smoothly when the external force is removed.

The controller I wrote is a joint torque-based controller, namely, the actuators I defined for the robot is motor-type actuators. The problem I encountered in simulation for both tasks is about how to plan the robot to stop after following a defined trajectory.

First, could you help me check my understandings in this problem. In my opinion, if I want the robot to stay at some configuations finally, suppose this joint configuration is q_end, the joint torque I need to applied to the robot could be calculated in this way:

self.data.qpos[0:self.joint_num] = q_end

self.data.qvel[0:self.joint_num] = np.zeros(self.joint_num)
self.data.qacc[0:self.joint_num] = np.zeros(self.joint_num)

mjo.mj_inverse(self.model, self.data)

return self.data.qfrc_inverse.copy()[0:self.joint_num]

mjo.mj_inverse(self.model, self.data)

torque = self.data.qfrc_inverse.copy()[0:self.joint_num]

or calculated in this way,

self.data.qpos[0:self.joint_num] = q_end

self.data.qvel[0:self.joint_num] = np.zeros(self.joint_num)
self.data.qacc[0:self.joint_num] = np.zeros(self.joint_num)
torque = self.data.qfrc_bias[:self.joint_num]

I guess the crucial part is to make sure the joint velocity and joint acceleration are all zeros at the end configuation, and then the joint torque calculated with qfrc_inverse or qfrc_bias could help the robot stay at end configuation. I tested these two methods in both tasks, for instance, right after the robot reaches the end point of the defined trajectory in task 1) and right after the external force is removed in task 2), I mean, just enforce the qvel and qacc to be all zeros and compute the torque. I found they can enforce the robot to stay static, just as what I applied at the reset state.

However, for instance, in task 2), what I am struggling with is after the external force is removed, the robot has inital joint velocities, joint accelerations and what I aim to do is to reduce the robot joint accelerations and velocities naturally to 0 so that the joint torque computed from the joint impedance controller right matches the joint torque computed from qfrc_inverse or qfrc_bias. In other words, for a joint impedance controller as follows,

I hope the desired joint velocities and accelerations all reaching 0 and the same as the current real joint velocities and accelerations, however, I found that even though I can successfully planned the desired joint velocities to 0, the small torque resulted from the error between the desired joint velocities/accelerations and current joint velocities/accelerations cannot ensure the robot to stay static.

Here I have several questions: 1) Should the joint velocities and joint accelerations strictly to be reduced to 0, namely, what if there would be some small difference, like 1E-5 or 1E-6 in order of magnitude, would some small joint torque cause the robot to move, like 0.0001 N*m? 2) Will the friction I set for each joint have a huge impact on my simulation performance.

Could you give me some good advice or tips I should definitely consider when doing such a motion plan task in mujoco?

Probably I ask too many details and "rookie" questions, anyway, really appreciate your help and look forward to hearing you.

Wishing you all the best.

Btw, may I ask a question about how long would a joint command last to control a robot, for instance, if there is a sudden joint torque reaching 10000 N.m at one step due to a sudden increase in acceleration, will this joint torque only last for, like 0.002s, the timestep.

Inverse dynamics can be used, as long as its applied on a frame-by-frame basis. Don't forget to store the kinematics before you overwrite them so you can restore them. Check out the LQR tutorial on an example use of inverse dynamics: https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/LQR.ipynb However, ID may not work well for all cases, and has limited physical plausibility when used on its own (when considering to apply it on a real robot)
The force is applied in an integrated way: https://mujoco.readthedocs.io/en/latest/computation/index.html#numerical-integration
If you set qfrc_applied or ctrl to some value, it will stay that way across steps. Be sure to clear them if you want the force to stop.
How much movement do you actually observe? Also consider there are passive/bias/gravity forces acting on your arm so after you rapidly freeze it in place, there will be some forces that will cause it to deviate slightly.
You could set up your controller so you don't have to have this extreme rigid stop once you reach your target. You are probably already familiar with the concept of damping ratio. If you set your controller gains to be critically damped you will not have a large overshoot that you need a drastic correction for: https://en.wikipedia.org/wiki/Damping, https://www.sciencedirect.com/topics/engineering/critical-damping

Thanks so much for you reply, Balint, really really appreciate your reply with so many details. Thanks very much for your time.

Actually, the movement I actually observe is quite large that I can obviously see that the robot keeps moving even though I plan the cartesian velocities and cartesian accelerations to decrease to zero with large damping ratio. The relationship between cartesian acceleration and cartesian velocity is governed by the differential equation below in the second task I mentioned above:

M * x_dot_dot + D * x_dot = 0 (0 indicates I remove the external force)

Then, I use to normal way to calculate joint velocities and joint accelerations with Jacobian matrix and it can be observed that the desired joint velocities and joint accelerations will reach 0 very fast which is the result I expect. However, there would always be some errors between the desired joint velocities/accelerations and the current joint velocities and accelerations which result in the unexpected large motion after I remove the external force. I also showed some data I printed in simulation in this issue #1911, which probably is the reason why I cannot get the qvel and qacc reduce to 0 quickly.

And yes, I also make sure it's a critically damped case, but the result is still not quite good which I think is veru counter-intuitive because I think all the theory is well implemented. So I suspect there must be something I miss in using mujoco for simulation.

Btw, you mentioned _"If you set qfrcapplied or ctrl to some value, it will stay that way across steps. Be sure to clear them if you want the force to stop." I am not sure whether I understand it in a right way, does it mean that,

1) If for every timestep (for instance, 0.001s), I set qfrc_applied or ctrl to some value, such a ctrl signal will stay across this timestep, namely, last for 0.001s, right? For the next step, I set qfrc_applied or ctrl to some different value, it will then stay across the next 0.001s because I need to update the joint torque every timestep according to the current joint pos/vel/acc and the desired qpos/qvel/qacc. 2) I only need to clear the joint torque when I don't want to apply any joint torque to a robot, right? I guess you don't mean that I need to clear it after I call mj_step func when I update the joint torque the way I mentioned above in 1)

Sorry for asking for so many details in the comment. Thanks very much again for your help, Balint.Wishing you all the best.

I meant that if e.g. you set qfrc_applied on the first frame, you will keep applying that force until you change it, for however many frames you run, not just across its single timestep.
If you use mj_inverse to calculate the force, that is the force needed to stop immediately in one frame. If you keep applying it after that frame then you will deviate from your position of course. So yes, once you want to stop applying a force, you have to clear it.

Really appreciate your time and patience, thanks very much, Balint.

Oh, I see, it is strongly related to the render frames at each time step. Then, how could I ensure that each time I call mj_step() function, it will only apply the ctrl input for one frame? I designed a test that I made the control input at each time step equals the gravity compensation torque.

I clipped the codes about timestep settings, rendering and step function as follows,

# timestep settings
self.model.opt.timestep = 0.001   

# create render 
from gymnasium.envs.mujoco.mujoco_rendering import MujocoRenderer
self.mujoco_renderer = MujocoRenderer(self.model, self.data, default_camera_config)

# call mj_step and and call render after each mj_step() call
self.data.ctrl = get_grav_compen_torques()
self.mujoco.mj_step(self.model, self.data)
self.mujoco.mj_forward(self.model, self.data)
self.mujoco_renderer.render(render_mode='human')

# code for get gravity_compensation_torque
def get_grav_compen_torques(self):
    self.data.qpos[0:self.joint_num] = self.data.qpos[0:self.joint_num].copy()
    self.data.qvel[0:self.joint_num] = np.zeros(self.joint_num)
    self.data.qacc[0:self.joint_num] = np.zeros(self.joint_num)
    mujoco.mj_inverse(self.model, self.data)
    return self.data.qfrc_inverse.copy()[0:self.joint_num]

I found that from the render window that it seems the robot keeps static, however, I printed the qpos, qvel, qacc after I call the mj_function and I found that the qvel and qacc are not zeros, so I guess the control torque actually lasts for more than one frame for each timestep, so how could I make sure the gravity compensation torque could enable the robot to stay at the initial position. May I ask what would be the best way to calculate the torque to help a robot maintain its configuration? What details should I pay attention to.

Thanks very much and wishing you all the best.

It would be very strange to only apply control for a single frame, it is much more common to apply it continuously and adapt its value (and therefore overwrite the previous values, ending their influence). In your torque function you showed, you need to copy the kinematics before you write to them so you can restore them. What values of qvel and qacc do you observe?

Also please refrain from starting multiple issues for the same/closely related question, feel free to edit or update an existing thread.


# code for get gravity_compensation_torque to perform kinematic changes in one step
def get_grav_compen_torques(self, desired_qvel, desired_qacc):
    qvel_copy = self.data.qvel.copy()
    qacc_copy = self.data.qacc.copy()

    self.data.qvel[0:self.joint_num] = desired_qvel
    self.data.qacc[0:self.joint_num] =desired_qacc
    mujoco.mj_inverse(self.model, self.data)
    frc = self.data.qfrc_inverse.copy()[0:self.joint_num]
    self.data.qvel = qvel_copy
    self.data.qacc = qacc_copy
    return frc

Hi, Balint,

I hope you're enjoying a nice day today!

Thanks for your reply and advice. I will pay attention to the way of submitting issues later.

Yeah, I added the kinematics copy codes in my function and I made a test that for every timestep, I just input the gravity torque as the control signal. And I recorded the qvel and qacc after I call the mj_step function as follows,

# step 0
qpos before mj_step [ 0.00e+00  6.04e-02 -1.63e-05 -1.37e+00 -3.44e-04  1.41e+00  2.11e-07]
qvel before mj_step [0. 0. 0. 0. 0. 0. 0.]
qacc before mj_step [0. 0. 0. 0. 0. 0. 0.]

qpos after mj_step [-1.50195170e-07  6.04015848e-02 -1.63310944e-05 -1.37001790e+00
 -3.40947640e-04  1.41001559e+00  2.04349517e-07]
qvel after mj_step [-1.50195170e-04  1.58480283e-03 -3.10944149e-05 -1.78954805e-02
  3.05235966e-03  1.55864222e-02 -6.65048268e-06]
qacc after mj_step [-1.51830029e-01  1.55795328e+00 -3.28263384e-02 -1.79627777e+01
  3.07997247e+00  1.57232104e+01 -5.75402593e-03]

# step 1
qpos before mj_step:  [-1.50195170e-07  6.04015848e-02 -1.63310944e-05 -1.37001790e+00
 -3.40947640e-04  1.41001559e+00  2.04349517e-07]
qvel before mj_step:  [-1.50195170e-04  1.58480283e-03 -3.10944149e-05 -1.78954805e-02
  3.05235966e-03  1.55864222e-02 -6.65048268e-06]
qacc before mj_step:  [-1.51320934e-01  1.58511516e+00 -2.97644474e-02 -1.78952125e+01
  3.05225710e+00  1.55869755e+01 -5.21298279e-03]

qpos after mj_step:  [-4.50081666e-07  6.04047814e-02 -1.63902512e-05 -1.37005362e+00
 -3.34870336e-04  1.41004662e+00  1.91592205e-07]
qvel after mj_step:  [-2.99886496e-04  3.19660525e-03 -5.91567782e-05 -3.57237921e-02
  6.07730422e-03  3.10376189e-02 -1.27573126e-05]
qacc after mj_step:  [-1.50581044e-01  1.61259126e+00 -2.69803772e-02 -1.78275442e+01
  3.02491574e+00  1.54524953e+01 -4.72373014e-03]

if I just call mj_step(model, data), it seems that the robot stay at its initial position as shown in the figure below, however, if I call mj_step(model, data, 100), it will fall down quickly as shown in the figure below,

which from another aspect proves that qvel and qacc are not 0 after apply this force. I set qvel and qacc = 0 before call mj_inverse and recover them after call mj_inverse and apply the calculated torque to control the robot for every time step.

Btw, there are more than 1 robot in my model, but I control them separately, for instance, self.data.ctrl[0:7] = tau1 for robot 1 and self.data.ctrl[7:] for another robot. For the current test, I just give the desired torque to the first robot for each timestep and give some random torques to the other robot. I guess the existence of a second robot in the model will not have influence in the non-zero qvel and non-zero qacc after applying the desired gravity compensation torque for the first robot.

google-deepmind / mujoco

How to plan the robot to stop perfectly after tracking a reference trajectory or moving for some time with external force applied in mujoco? #1909