The wrong feedback of franka Panda's Fk and its F/T sensor

[HongyuanChen]

System Info

Ubuntu 20.04
Robosuite 1.5

Information

When I try to use Robosuite to do the Reinforcement Learning, I focus on the existing environment in manipulation folder. However, I encountered problems about the wrong feedback of Panda(its default gripper:panda hand). When I do the action to make the EEF continuously downward until the robot touch the desk. I get the EEF_pos and F/Tsensor data from eef_pos = np.array(env.robot._hand_pos["right"]) and eef_force = np.array(self.robot.ee_force["right"]). When I run the scipts, the robot does the action,but the eef_pos and eef_force remain consistent.
(Note: the eef_force seems have a bias the ForceZ remain -4N]

Reproduction

panda_pickplace_env.py:

`from robosuite import make
from robosuite import load_composite_controller_config

from robosuite.wrappers.gym_wrapper import GymWrapper
import gym
import numpy as np
from gym import spaces
from robosuite.controllers.composite.composite_controller_factory import load_composite_controller_config
import random

import robosuite.robots.robot
class PandaPickPlaceRLWrapper(gym.Env):
def init(self, has_renderer=False, camera_name="agentview", camera_size=(84, 84), control_delta_clip=0.05):
super().init()
self.camera_name = camera_name
self.camera_size = camera_size
self.control_delta_clip = control_delta_clip

    controller_config = load_composite_controller_config(controller="BASIC",robot="Panda")
    controller_config["use_sensors"] = True
    
    self.env = make(
        env_name="PickPlaceCan",
        robots="Panda",
        controller_configs=controller_config,
        has_renderer=has_renderer,
        has_offscreen_renderer=True,
        use_camera_obs=True,
        use_object_obs=False,
        reward_shaping=True,
        reward_scale = 1.0,
        control_freq=20,
        camera_names=[camera_name],
        camera_heights=camera_size[1],
        camera_widths=camera_size[0],
        render_camera=camera_name,
    )

    self.sim = self.env.sim
    self.control_freq = self.env.control_freq
    self.robot = self.env.robots[0]
    obs = self.env.reset()
    self.obs_img_shape = (3,) + camera_size

    # new
    self.force_torque_offset = None
    self._calibrate_sensors()

    self.observation_space = spaces.Dict({
        "image": spaces.Box(0, 255, shape=self.obs_img_shape, dtype=np.uint8),
        "joint_pos": spaces.Box(low=-np.pi,high = np.pi,shape=(7,),dtype=np.float32),
        "force_torque": spaces.Box(low=-np.inf, high=np.inf, shape=(6,), dtype=np.float32)
    })

    self.action_space = spaces.Box(low=-self.control_delta_clip, high=self.control_delta_clip, shape=(7,), dtype=np.float32)
    self.total_reward = 0
    self.episode_rewards = []

# new
def _calibrate_sensors(self):
    """校准力/力矩传感器"""
    obs = self.env.reset()
    try:
        force_data = self.sim.data.sensor("gripper0_right_force_ee")
        torque_data = self.sim.data.sensor("gripper0_right_torque_ee")
        print(f"MuJoCo Sensor: Force: {force_data}, Torque: {torque_data}")
    except ValueError as e:
        print(f"Sensor Error: {e}")
    eef_force = np.array(self.robot.ee_force["right"])
    eef_torque = np.array(self.robot.ee_torque["right"])
    self.force_torque_offset = np.concatenate([eef_force, eef_torque])
    print(f"Sensor offset: {self.force_torque_offset}")

def reset(self):
    # new
    self.robot.set_robot_joint_positions([00, -0.5, 0, -2.0, 0, 1.5, 0.785])
    obs = self.env.reset()
    self.total_reward = 0
    return self._obs_to_dict(obs)

def step(self, action):
    action = np.clip(action, self.action_space.low, self.action_space.high)
    # current_qpos = self.robot.get_joint_positions()
    # new_qpos = current_qpos + action
    # self.robot.set_joint_positions(new_qpos)
    obs, reward, done, info = self.env.step(action)
    self.total_reward += reward
    if done:
        self.episode_rewards.append(self.total_reward)
    return self._obs_to_dict(obs), reward, done, info

def _obs_to_dict(self, obs):
    image = obs[f"{self.camera_name}_image"].astype(np.uint8)
    # (H,W,C)-->(C,H,W)
    image = np.transpose(image,(2,0,1))
    joint_pos = obs["robot0_joint_pos"]
    if "robot0_eef_force" in obs and "robot0_eef_torque" in obs:
        eef_force = obs["robot0_eef_force"]
        eef_torque = obs["robot0_eef_torque"]
    else:
        eef_force = np.array(self.robot.ee_force["right"])
        eef_torque = np.array(self.robot.ee_torque["right"])
    force_torque = np.concatenate([eef_force,eef_torque])
    # state = np.concatenate([joint_pos, eef_force, eef_torque]).astype(np.float32)
    return {"image": image, "joint_pos": joint_pos,"force_torque":force_torque}

def render(self, mode="human"):
    self.env.render()

def close(self):
    self.env.close()

def get_episode_rewards(self):
    return self.episode_rewards

def seed(self,seed=None):
    if seed is not None:
        random.seed(seed)
        np.random.seed(seed)
        self.sim.model.seed = seed
    return [seed]`

test_force_torque.py:

`import robosuite as suite
from panda_pickplace_env import PandaPickPlaceRLWrapper
import numpy as np
import matplotlib.pyplot as plt
import time
import os

env = PandaPickPlaceRLWrapper(has_renderer=True)
obs = env.reset()

steps = []
eef_positions = []
force_torques = []
contacts = []

for step in range(1000):
action = np.zeros(7)
action[2] = -0.05
action[-1] = 0.0
obs, reward, done, info = env.step(action)

eef_pos = np.array(env.robot._hand_pos["right"])
force_torque = obs["force_torque"]
contact = any(c.dist <= 0 for c in env.sim.data.contact[:env.sim.data.ncon])

steps.append(step)
eef_positions.append(eef_pos)
force_torques.append(force_torque)
contacts.append(contact)

env.render()
time.sleep(0.01)  
if done:
    obs = env.reset()

env.close()

eef_positions = np.array(eef_positions) # (200, 3)
force_torques = np.array(force_torques) # (200, 6)
contacts = np.array(contacts) # (200,)

plt.figure(figsize=(12, 8))

plt.subplot(2, 1, 1)
plt.plot(steps, eef_positions[:, 0], label='X', color='r')
plt.plot(steps, eef_positions[:, 1], label='Y', color='g')
plt.plot(steps, eef_positions[:, 2], label='Z', color='b')
plt.title('EEF Position vs. Time Step')
plt.xlabel('Time Step')
plt.ylabel('Position (m)')
plt.legend()
plt.grid(True)

contact_steps = np.where(contacts)[0]
if len(contact_steps) > 0:
plt.axvline(x=contact_steps[0], color='k', linestyle='--', label='First Contact')
plt.legend()

plt.subplot(2, 1, 2)
labels = ['Fx', 'Fy', 'Fz', 'Tx', 'Ty', 'Tz']
colors = ['r', 'g', 'b', 'c', 'm', 'y']
for i in range(6):
plt.plot(steps, force_torques[:, i], label=labels[i], color=colors[i])
plt.title('Force/Torque vs. Time Step')
plt.xlabel('Time Step')
plt.ylabel('Force (N) / Torque (Nm)')
plt.legend()
plt.grid(True)

if len(contact_steps) > 0:
plt.axvline(x=contact_steps[0], color='k', linestyle='--', label='First Contact')
plt.legend()

plt.tight_layout()
output_dir = "plots"
os.makedirs(output_dir, exist_ok=True)
plt.savefig(os.path.join(output_dir, "eef_pos_force_torque.png"))
plt.close()

data = np.hstack([np.array(steps)[:, None], eef_positions, force_torques, contacts[:, None]])
np.savetxt(os.path.join(output_dir, "test_data.csv"), data, delimiter=",",
header="Step,X,Y,Z,Fx,Fy,Fz,Tx,Ty,Tz,Contact", comments="")`

Expected behavior

No response