Incorrect values with dof forces

Hi, there!

I’m trying to measure an external force that applied to a joint leveraging gym.acquire_dof_force_tensor(). But the returned value is pretty different from what I expected.

Specifically, I import an actor with two links tin to the environment, and fix their base pose. The imported two links are connected with a prismatic type joint, the axis is along to z axis. After that, I apply a force (0, 0, F) to the upper link.

Finally, I use acquire_dof_force_tensor() to query the force of the prismatic joint, however the return value is a very small value whatever the value I set to F.

In my assumption, the returned value should be F - (gravity of the upper link), Is that right?

environment like this:

The returned value:
2023-12-14 17-08-48 的屏幕截图

The Code :

device = "cuda:0"
args = gymutil.parse_arguments(description="test",
                               custom_parameters=[
                                   {"name": "--num_envs", "type": int, "default": 256, "help": "Number of environments to create"},
                                   {"name": "--pos_control", "type": gymutil.parse_bool, "const": True, "default": True, "help": "Trace circular path in XZ plane"},
                                   {"name": "--orn_control", "type": gymutil.parse_bool, "const": True, "default": False, "help": "Send random orientation commands"}])


gym = gymapi.acquire_gym()
sim_params = gymapi.SimParams()
sim_params.up_axis = gymapi.UP_AXIS_Z
sim_params.gravity = gymapi.Vec3(0.0, 0.0, -9.8)
sim_params.dt = 1.0 / 60.0
sim_params.substeps = 2
physics_engine = gymapi.SIM_PHYSX
if physics_engine == gymapi.SIM_PHYSX:
    sim_params.physx.solver_type = 1
    sim_params.physx.num_position_iterations = 4
    sim_params.physx.num_velocity_iterations = 1
    sim_params.physx.num_threads = 0
    sim_params.physx.use_gpu = True

sim_params.use_gpu_pipeline = True

sim = gym.create_sim(args.compute_device_id, args.graphics_device_id, args.physics_engine, sim_params)

if sim is None:
    raise Exception("Failed to create sim")

viewer = gym.create_viewer(sim, gymapi.CameraProperties())
if viewer is None:
    raise Exception("Failed to create viewer")

# Add ground plane
plane_params = gymapi.PlaneParams()
plane_params.normal = gymapi.Vec3(0, 0, 1)
gym.add_ground(sim, plane_params)
asset_root = "/mnt/data/pyCharm-project/Reinforcement_Learning/RL_assembly/assets"

joint_asset_options = gymapi.AssetOptions()
joint_asset_options.flip_visual_attachments = False
joint_asset_options.fix_base_link = True
joint_asset_options.disable_gravity = False
joint_asset_options.thickness = 0.001
joint_asset_options.angular_damping = 0.01
joint_asset_file = "urdf/rokae_description/multi_joint.urdf"
joint_asset = gym.load_asset(sim, asset_root, joint_asset_file, joint_asset_options)

joint_pose = gymapi.Transform()
joint_pose.p = gymapi.Vec3(0, 0.6, 0.1)
joint_pose.r = gymapi.Quat(0, 0, 0, 1)

joint_dof_props = gym.get_asset_dof_properties(joint_asset)
joint_dof_props["driveMode"][:].fill(gymapi.DOF_MODE_EFFORT)
joint_dof_props["stiffness"][:].fill(100.0)
joint_dof_props["damping"][:].fill(0.0)
num_envs = 1
num_per_row = int(math.sqrt(num_envs))
spacing = 1.0
env_lower = gymapi.Vec3(-spacing, -spacing, 0.0)
env_upper = gymapi.Vec3(spacing, spacing, spacing)

# default franka pose
pose = gymapi.Transform()
pose.p = gymapi.Vec3(0, 0, 0)
pose.r = gymapi.Quat(0, 0, 0, 1)

print("Creating %d environments" % num_envs)

envs = []
hand_idxs = []
init_pos_list = []
init_orn_list = []

for i in range(num_envs):
    # Create env
    env = gym.create_env(sim, env_lower, env_upper, num_per_row)
    envs.append(env)

    mjoint_handle = gym.create_actor(env, joint_asset, joint_pose, "mjoint", i, 1)
    gym.set_actor_dof_properties(env, mjoint_handle, joint_dof_props)
    gym.enable_actor_dof_force_sensors(env, mjoint_handle)
    z_indx = gym.find_actor_rigid_body_index(env, mjoint_handle, "z_link", gymapi.DOMAIN_ENV)
    z_handle = gym.find_actor_rigid_body_handle(env, mjoint_handle, "z_link")

grid_count = gym.get_env_rigid_body_count(envs[0])
gym.prepare_sim(sim)

force = gymapi.Vec3(0, 0, 1000.0)
torque_amt = 100
itr = 0
while not gym.query_viewer_has_closed(viewer):
    itr += 1
    forces = torch.zeros((num_envs, grid_count, 3), device=device, dtype=torch.float)
    torques = torch.zeros((num_envs, grid_count, 3), device=device, dtype=torch.float)
    forces[:, z_indx, 2] = 100
    gym.apply_rigid_body_force_tensors(sim, gymtorch.unwrap_tensor(forces), gymtorch.unwrap_tensor(torques), gymapi.ENV_SPACE)



    # Update jacobian and mass matrix
    gym.refresh_rigid_body_state_tensor(sim)
    gym.refresh_dof_state_tensor(sim)
    gym.refresh_jacobian_tensors(sim)
    gym.refresh_mass_matrix_tensors(sim)
    gym.refresh_dof_force_tensor(sim)

    joint_effort = gym.acquire_dof_force_tensor(sim)
    joint_eff_data = gymtorch.wrap_tensor(joint_effort).view(num_envs, -1)
    print(joint_eff_data)


   
    
    # Step the physics
    gym.simulate(sim)
    gym.fetch_results(sim, True)

    # Step rendering
    gym.step_graphics(sim)
    gym.draw_viewer(viewer, sim, False)

The imported urdf :

<?xml version="1.0" ?>
<robot name="multi_joint">

  <link name="world"/>
  
  <link name="base_link">
    <visual>
      <geometry>
        <mesh filename="meshes/visual/fixture_box.obj" scale="0.2 0.2 0.2"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="white"/>
    </visual>
    <inertial>
        <density value="1000"/>
    </inertial>
    <collision>
      <geometry>
        <mesh filename="meshes/visual/fixture_box.obj" scale="0.2 0.2 0.2"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>

  <link name="z_link">
    <visual>
      <geometry>
        <mesh filename="meshes/visual/fixture_box.obj" scale="0.15 0.15 0.15"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="white"/>
    </visual>
    <inertial>
        <density value="1000"/>
    </inertial>
    <collision>
      <geometry>
        <mesh filename="meshes/visual/fixture_box.obj" scale="0.15 0.15 0.15"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>

  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
  </joint>

  <joint name="z_joint" type="prismatic">
    <parent link="base_link"/>
    <child link="z_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.2"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="10.0"/>
    <limit effort="100" lower="0.0" upper="0.1" velocity="0.2"/>
    <!-- <mimic joint="panda_finger_joint1"/> -->
  </joint>
</robot>