Oscillating force sensor data when sending signals in a control loop #1953

zichunxx · 2024-08-15T11:33:17Z

zichunxx
Aug 15, 2024

Hi,

When I sent signals to the manipulator in a control loop to scale the end-effector displacement, I found that the force sensor data was oscillating. I made a minimal example to illustrate this phenomenon based on mjctrl.

Below is the modified iiwa14.xml and all the other assets can be found in there.

<mujoco model="iiwa14">
  <compiler angle="radian" meshdir="assets" autolimits="true"/>

  <option integrator="implicitfast"/>

  <default>
    <default class="iiwa">
      <default class="visual">
        <geom type="mesh" contype="0" conaffinity="0" group="2" material="gray"/>
      </default>
      <default class="collision">
        <geom group="3"/>
      </default>
      <joint axis="0 0 1"/>
      <default class="joint1">
        <joint range="-2.96706 2.96706"/>
        <default class="joint2">
          <joint range="-2.0944 2.0944"/>
        </default>
      </default>
      <default class="joint3">
        <joint range="-3.05433 3.05433"/>
      </default>
    </default>
  </default>

  <asset>
    <material name="gray" rgba="0.4 0.4 0.4 1"/>
    <material name="light_gray" rgba="0.6 0.6 0.6 1"/>
    <material name="black" rgba="0 0 0 1"/>
    <material name="orange" rgba="1 0.423529 0.0392157 1"/>

    <mesh file="link_0.obj"/>
    <mesh file="link_1.obj"/>
    <mesh file="link_2_orange.obj"/>
    <mesh file="link_2_grey.obj"/>
    <mesh file="link_3.obj"/>
    <mesh file="band.obj"/>
    <mesh file="kuka.obj"/>
    <mesh file="link_4_orange.obj"/>
    <mesh file="link_4_grey.obj"/>
    <mesh file="link_5.obj"/>
    <mesh file="link_6_orange.obj"/>
    <mesh file="link_6_grey.obj"/>
    <mesh file="link_7.obj"/>
  </asset>

  <worldbody>
    <light name="top" pos="0 0 2" mode="trackcom"/>
    <body name="base" childclass="iiwa">
      <inertial mass="5" pos="-0.1 0 0.07" diaginertia="0.05 0.06 0.03"/>
      <geom class="visual" mesh="link_0"/>
      <geom class="collision" size="0.12" pos="0 0 0.03"/>
      <geom class="collision" size="0.08" pos="-0.08 0 0.103"/>
      <geom class="collision" size="0.08" pos="-0.08 0 0.04"/>
      <geom class="collision" size="0.1" pos="0 0 0.14"/>
      <body name="link1" pos="0 0 0.1575">
        <inertial mass="5.76" pos="0 -0.03 0.12" diaginertia="0.0333 0.033 0.0123"/>
        <joint name="joint1" class="joint1"/>
        <geom class="visual" mesh="link_1"/>
        <geom class="collision" size="0.08" pos="0 0 -0.0005"/>
        <geom class="collision" size="0.075" pos="0.01 -0.025 0.0425"/>
        <geom class="collision" size="0.075" pos="-0.01 -0.025 0.0425"/>
        <geom class="collision" size="0.07" pos="0.01 -0.045 0.1025"/>
        <geom class="collision" size="0.07" pos="-0.01 -0.045 0.1025"/>
        <body name="link2" pos="0 0 0.2025" quat="0 0 1 1">
          <inertial mass="6.35" pos="0.0003 0.059 0.042" diaginertia="0.0305 0.0304 0.011" quat="0 0 1 1"/>
          <joint name="joint2" class="joint2"/>
          <geom class="visual" material="orange" mesh="link_2_orange"/>
          <geom class="visual" mesh="link_2_grey"/>
          <geom class="collision" size="0.095" pos="0 0 -0.01"/>
          <geom class="collision" size="0.09" pos="0 0 0.045"/>
          <geom class="collision" size="0.07" pos="-0.01 0.04 0.054"/>
          <geom class="collision" size="0.065" pos="-0.01 0.09 0.04"/>
          <geom class="collision" size="0.065" pos="-0.01 0.13 0.02"/>
          <geom class="collision" size="0.07" pos="0.01 0.04 0.054"/>
          <geom class="collision" size="0.065" pos="0.01 0.09 0.04"/>
          <geom class="collision" size="0.065" pos="0.01 0.13 0.02"/>
          <geom class="collision" size="0.075" pos="0 0.18 0"/>
          <body name="link3" pos="0 0.2045 0" quat="0 0 1 1">
            <inertial mass="3.5" pos="0 0.03 0.13" diaginertia="0.025 0.0238 0.0076"/>
            <joint name="joint3" class="joint1"/>
            <geom class="visual" mesh="link_3"/>
            <geom class="visual" material="light_gray" mesh="band"/>
            <geom class="visual" material="black" mesh="kuka"/>
            <geom class="collision" size="0.075" pos="0 0 0.0355"/>
            <geom class="collision" size="0.06" pos="0.01 0.023 0.0855"/>
            <geom class="collision" size="0.055" pos="0.01 0.048 0.1255"/>
            <geom class="collision" size="0.06" pos="0.01 0.056 0.1755"/>
            <geom class="collision" size="0.06" pos="-0.01 0.023 0.0855"/>
            <geom class="collision" size="0.055" pos="-0.01 0.048 0.1255"/>
            <geom class="collision" size="0.06" pos="-0.01 0.056 0.1755"/>
            <geom class="collision" size="0.075" pos="0 0.045 0.2155"/>
            <geom class="collision" size="0.075" pos="0 0 0.2155"/>
            <body name="link4" pos="0 0 0.2155" quat="1 1 0 0">
              <inertial mass="3.5" pos="0 0.067 0.034" diaginertia="0.017 0.0164 0.006" quat="1 1 0 0"/>
              <joint name="joint4" class="joint2"/>
              <geom class="visual" material="orange" mesh="link_4_orange"/>
              <geom class="visual" mesh="link_4_grey"/>
              <geom class="collision" size="0.078" pos="0 0.01 0.046"/>
              <geom class="collision" size="0.06" pos="0.01 0.06 0.052"/>
              <geom class="collision" size="0.065" pos="0.01 0.12 0.034"/>
              <geom class="collision" size="0.06" pos="-0.01 0.06 0.052"/>
              <geom class="collision" size="0.065" pos="-0.01 0.12 0.034"/>
              <geom class="collision" size="0.075" pos="0 0.184 0"/>
              <body name="link5" pos="0 0.1845 0" quat="0 0 1 1">
                <inertial mass="3.5" pos="0.0001 0.021 0.076" diaginertia="0.01 0.0087 0.00449"/>
                <joint name="joint5" class="joint1"/>
                <geom class="visual" mesh="link_5"/>
                <geom class="visual" material="light_gray" mesh="band"/>
                <geom class="visual" material="black" mesh="kuka"/>
                <geom class="collision" size="0.075" pos="0 0 0.0335"/>
                <geom class="collision" size="0.05" pos="-0.012 0.031 0.0755"/>
                <geom class="collision" size="0.05" pos="0.012 0.031 0.0755"/>
                <geom class="collision" size="0.04" pos="-0.012 0.06 0.1155"/>
                <geom class="collision" size="0.04" pos="0.012 0.06 0.1155"/>
                <geom class="collision" size="0.04" pos="-0.01 0.065 0.1655"/>
                <geom class="collision" size="0.04" pos="0.01 0.065 0.1655"/>
                <geom class="collision" size="0.035" pos="-0.012 0.065 0.1855"/>
                <geom class="collision" size="0.035" pos="0.012 0.065 0.1855"/>
                <body name="link6" pos="0 0 0.2155" quat="1 1 0 0">
                  <inertial mass="1.8" pos="0 0.0006 0.0004" diaginertia="0.0049 0.0047 0.0036" quat="1 1 0 0"/>
                  <joint name="joint6" class="joint2"/>
                  <geom class="visual" material="orange" mesh="link_6_orange"/>
                  <geom class="visual" mesh="link_6_grey"/>
                  <geom class="collision" size="0.055" pos="0 0 -0.059"/>
                  <geom class="collision" size="0.065" pos="0 -0.03 0.011"/>
                  <geom class="collision" size="0.08"/>
                  <body name="link7" pos="0 0.081 0" quat="0 0 1 1">
                    <inertial mass="1.2" pos="0 0 0.02" diaginertia="0.001 0.001 0.001"/>
                    <joint name="joint7" class="joint3"/>
                    <geom class="visual" mesh="link_7"/>
                    <geom class="collision" size="0.06" pos="0 0 0.001"/>
                    <body name="attachment" pos="0 0 0.045" quat="0 0 0 1">
                      <site name="attachment_site" rgba="1 0 0 1" size="0.01" group="1"/>
                      <geom type="cylinder" size="0.044 0.02" rgba="0.66 0.66 0.66 1"/>
                    </body>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
  </worldbody>

  <contact>
    <exclude body1="base" body2="link1"/>
    <exclude body1="base" body2="link2"/>
    <exclude body1="base" body2="link3"/>
    <exclude body1="link1" body2="link3"/>
    <exclude body1="link3" body2="link5"/>
    <exclude body1="link4" body2="link7"/>
    <exclude body1="link5" body2="link7"/>
  </contact>

  <actuator>
    <motor name="joint1" joint="joint1" ctrlrange="-320 320"/>
    <motor name="joint2" joint="joint2" ctrlrange="-320 320"/>
    <motor name="joint3" joint="joint3" ctrlrange="-176 176"/>
    <motor name="joint4" joint="joint4" ctrlrange="-176 176"/>
    <motor name="joint5" joint="joint5" ctrlrange="-110 110"/>
    <motor name="joint6" joint="joint6" ctrlrange="-40 40"/>
    <motor name="joint7" joint="joint7" ctrlrange="-40 40"/>
  </actuator>

  <keyframe>
    <key name="home" qpos="0 0 0 -1.5708 0 1.5708 0"/>
  </keyframe>

  <sensor>
    <force name="force_ee" site="attachment_site"/>
    <torque name="torque_ee" site="attachment_site"/>
  </sensor>

</mujoco>

Here is the test python file:

import mujoco
import mujoco.viewer
import numpy as np
import time
import matplotlib.pyplot as plt

# Cartesian impedance control gains.
impedance_pos = np.asarray([100.0, 100.0, 100.0])  # [N/m]
impedance_ori = np.asarray([150.0, 150.0, 150.0])  # [Nm/rad]

# Joint impedance control gains.
Kp_null = np.asarray([75.0, 75.0, 50.0, 50.0, 40.0, 25.0, 25.0])

# Damping ratio for both Cartesian and joint impedance control.
damping_ratio = 1.0

# Gains for the twist computation. These should be between 0 and 1. 0 means no
# movement, 1 means move the end-effector to the target in one integration step.
Kpos: float = 0.95

# Gain for the orientation component of the twist computation. This should be
# between 0 and 1. 0 means no movement, 1 means move the end-effector to the target
# orientation in one integration step.
Kori: float = 0.95

# Integration timestep in seconds.
integration_dt: float = 1.0

# Whether to enable gravity compensation.
gravity_compensation: bool = True

# Simulation timestep in seconds.
dt: float = 0.002


def main() -> None:
    assert mujoco.__version__ >= "3.1.0", "Please upgrade to mujoco 3.1.0 or later."

    # Load the model and data.
    model = mujoco.MjModel.from_xml_path("kuka_iiwa_14/scene.xml")
    data = mujoco.MjData(model)

    model.opt.timestep = dt

    # Compute damping and stiffness matrices.
    damping_pos = damping_ratio * 2 * np.sqrt(impedance_pos)
    damping_ori = damping_ratio * 2 * np.sqrt(impedance_ori)
    Kp = np.concatenate([impedance_pos, impedance_ori], axis=0)
    Kd = np.concatenate([damping_pos, damping_ori], axis=0)
    Kd_null = damping_ratio * 2 * np.sqrt(Kp_null)

    # End-effector site we wish to control.
    site_name = "attachment_site"
    site_id = model.site(site_name).id

    # Get the dof and actuator ids for the joints we wish to control. These are copied
    # from the XML file. Feel free to comment out some joints to see the effect on
    # the controller.
    joint_names = [
        "joint1",
        "joint2",
        "joint3",
        "joint4",
        "joint5",
        "joint6",
        "joint7",
    ]
    dof_ids = np.array([model.joint(name).id for name in joint_names])
    actuator_ids = np.array([model.actuator(name).id for name in joint_names])

    # Initial joint configuration saved as a keyframe in the XML file.
    key_name = "home"
    key_id = model.key(key_name).id
    q0 = model.key(key_name).qpos

    # Mocap body we will control with our mouse.
    mocap_name = "target"
    mocap_id = model.body(mocap_name).mocapid[0]

    # Pre-allocate numpy arrays.
    jac = np.zeros((6, model.nv))
    twist = np.zeros(6)
    site_quat = np.zeros(4)
    site_quat_conj = np.zeros(4)
    error_quat = np.zeros(4)
    M_inv = np.zeros((model.nv, model.nv))
    Mx = np.zeros((6, 6))

    init_mocap_pos = data.mocap_pos[mocap_id].copy()
    steps = 1000
    down_traj = list(np.linspace(init_mocap_pos[2], 0.1, steps))
    counter = 0

    force_data = []
    torque_data = []

    def loop_step(target_pos, force_data, torque_data):
        control_freq = 10  # Hz
        Kpos = 1 / control_freq

        for _ in range(control_freq):
            data.mocap_pos[mocap_id][2] = target_pos
            # Spatial velocity (aka twist).
            dx = data.mocap_pos[mocap_id] - data.site(site_id).xpos
            twist[:3] = Kpos * dx / integration_dt
            mujoco.mju_mat2Quat(site_quat, data.site(site_id).xmat)
            mujoco.mju_negQuat(site_quat_conj, site_quat)
            mujoco.mju_mulQuat(error_quat, data.mocap_quat[mocap_id], site_quat_conj)
            mujoco.mju_quat2Vel(twist[3:], error_quat, 1.0)
            twist[3:] *= Kori / integration_dt

            # Jacobian.
            mujoco.mj_jacSite(model, data, jac[:3], jac[3:], site_id)

            # Compute the task-space inertia matrix.
            mujoco.mj_solveM(model, data, M_inv, np.eye(model.nv))
            Mx_inv = jac @ M_inv @ jac.T
            if abs(np.linalg.det(Mx_inv)) >= 1e-2:
                Mx = np.linalg.inv(Mx_inv)
            else:
                Mx = np.linalg.pinv(Mx_inv, rcond=1e-2)

            # Compute generalized forces.
            tau = jac.T @ Mx @ (Kp * twist - Kd * (jac @ data.qvel[dof_ids]))

            # Add joint task in nullspace.
            Jbar = M_inv @ jac.T @ Mx
            ddq = Kp_null * (q0 - data.qpos[dof_ids]) - Kd_null * data.qvel[dof_ids]
            tau += (np.eye(model.nv) - jac.T @ Jbar.T) @ ddq

            # Add gravity compensation.
            if gravity_compensation:
                tau += data.qfrc_bias[dof_ids]

            # Set the control signal and step the simulation.
            np.clip(tau, *model.actuator_ctrlrange.T, out=tau)
            data.ctrl[actuator_ids] = tau[actuator_ids]
            mujoco.mj_step(model, data)

            force_data.append(data.sensor("force_ee").data.copy())
            torque_data.append(data.sensor("torque_ee").data.copy())

    def single_step(target_pos, force_data, torque_data):
        data.mocap_pos[mocap_id][2] = target_pos
        # Spatial velocity (aka twist).
        dx = data.mocap_pos[mocap_id] - data.site(site_id).xpos
        twist[:3] = Kpos * dx / integration_dt
        mujoco.mju_mat2Quat(site_quat, data.site(site_id).xmat)
        mujoco.mju_negQuat(site_quat_conj, site_quat)
        mujoco.mju_mulQuat(error_quat, data.mocap_quat[mocap_id], site_quat_conj)
        mujoco.mju_quat2Vel(twist[3:], error_quat, 1.0)
        twist[3:] *= Kori / integration_dt

        # Jacobian.
        mujoco.mj_jacSite(model, data, jac[:3], jac[3:], site_id)

        # Compute the task-space inertia matrix.
        mujoco.mj_solveM(model, data, M_inv, np.eye(model.nv))
        Mx_inv = jac @ M_inv @ jac.T
        if abs(np.linalg.det(Mx_inv)) >= 1e-2:
            Mx = np.linalg.inv(Mx_inv)
        else:
            Mx = np.linalg.pinv(Mx_inv, rcond=1e-2)

        # Compute generalized forces.
        tau = jac.T @ Mx @ (Kp * twist - Kd * (jac @ data.qvel[dof_ids]))

        # Add joint task in nullspace.
        Jbar = M_inv @ jac.T @ Mx
        ddq = Kp_null * (q0 - data.qpos[dof_ids]) - Kd_null * data.qvel[dof_ids]
        tau += (np.eye(model.nv) - jac.T @ Jbar.T) @ ddq

        # Add gravity compensation.
        if gravity_compensation:
            tau += data.qfrc_bias[dof_ids]

        # Set the control signal and step the simulation.
        np.clip(tau, *model.actuator_ctrlrange.T, out=tau)
        data.ctrl[actuator_ids] = tau[actuator_ids]
        mujoco.mj_step(model, data)

        force_data.append(data.sensor("force_ee").data.copy())
        torque_data.append(data.sensor("torque_ee").data.copy())

    with mujoco.viewer.launch_passive(
        model=model,
        data=data,
        show_left_ui=False,
        show_right_ui=False,
    ) as viewer:
        # Reset the simulation.
        mujoco.mj_resetDataKeyframe(model, data, key_id)

        # Reset the free camera.
        mujoco.mjv_defaultFreeCamera(model, viewer.cam)

        # Enable site frame visualization.
        viewer.opt.frame = mujoco.mjtFrame.mjFRAME_SITE
        while viewer.is_running():

            if counter < steps:
                step_start = time.time()

                loop_step(target_pos=down_traj[counter], force_data=force_data, torque_data=torque_data)
                # single_step(target_pos=down_traj[counter], force_data=force_data, torque_data=torque_data)

                counter += 1
            else:
                break

            viewer.sync()
            time_until_next_step = dt - (time.time() - step_start)
            if time_until_next_step > 0:
                time.sleep(time_until_next_step)

        # plot
        fig, (ax1, ax2) = plt.subplots(1, 2)
        ax1.plot(np.asarray(range(len(force_data))), np.asarray(force_data)[:, 0], label="x_force")
        ax1.plot(np.asarray(range(len(force_data))), np.asarray(force_data)[:, 1], label="y_force")
        ax1.plot(np.asarray(range(len(force_data))), np.asarray(force_data)[:, 2], label="z_force")
        ax1.legend(loc="upper right")

        ax2.plot(np.asarray(range(len(torque_data))), np.asarray(torque_data)[:, 0], label="x_torque")
        ax2.plot(np.asarray(range(len(torque_data))), np.asarray(torque_data)[:, 1], label="y_torque")
        ax2.plot(np.asarray(range(len(torque_data))), np.asarray(torque_data)[:, 2], label="z_torque")
        ax2.legend(loc="upper right")

        plt.show()


if __name__ == "__main__":
    main()

When I send the signal with the loop_step function, the required force data is shown below, and the oscillation can be found after zoom-in.

However, when I switch to the single_step function (i.e., the official example given by @kevinzakka), the oscillation can not be found even after zoom-in.

I think this kind of oscillation should not happen when no additional noise is added. Is the problem caused by the way of getting the force sensor? How can I eliminate this oscillation and still send the signal in a control loop?

Thanks for your help!

lucky-luke-98 · 2024-09-02T11:26:58Z

lucky-luke-98
Sep 2, 2024

Hi,

I am sorry, if I am not able to help with your problem, but I have a question about your task of the impedance controller. Since I have a similar contact-rich setup and I am experiencing a different behavior of the force sensor, I would be interested in how fast you are controlling your manipulator (assuming your task is wiping)?
I always seem to have a tradeoff between my target velocity and force. When I increase my velocity to the target velocity, I lose contact and therefore force with my object leading to a skipping behavior.

To your question:
I am relatively new to mujoco, but I am assuming that the force oscillation originates from how contact is computed in mujoco. This could be solved by specifying different stiffness coefficients of your contact objects. Here, I refer to the documentation

1 reply

zichunxx Sep 7, 2024
Author

Hi, sorry for the late reply!

For the example above, the end-effector follows a path in the Cartesian space without any contact. I'm not sure I understand this correctly. I think this kind of oscillation should not happen in the simulation environment without contact. The strange thing is that the oscillation occurs when sending control commands in a loop.

Thanks for your kind help!

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Oscillating force sensor data when sending signals in a control loop #1953

{{title}}

Replies: 1 comment 1 reply

{{title}}

{{title}}

Select a reply

Oscillating force sensor data when sending signals in a control loop #1953

zichunxx Aug 15, 2024

Replies: 1 comment · 1 reply

lucky-luke-98 Sep 2, 2024

zichunxx Sep 7, 2024 Author

zichunxx
Aug 15, 2024

Replies: 1 comment 1 reply

lucky-luke-98
Sep 2, 2024

zichunxx Sep 7, 2024
Author