Robot doesn't respond to action in tor (torque) mode #230
Description
Hello @maxspahn, I think there's an issue with torque mode in robots. take this code for example:
import gymnasium as gym
from urdfenvs.urdf_common.urdf_env import UrdfEnv
from urdfenvs.robots.generic_urdf import GenericUrdfReacher
from urdfenvs.sensors.full_sensor import FullSensor
from urdfenvs.scene_examples.goal import goal1
import numpy as np
robots = [
GenericUrdfReacher(urdf="pointRobot.urdf", mode="tor"), # MODE IS SET HERE!
]
render: bool = True
env = UrdfEnv(render=render, robots=robots)
env.add_goal(goal1)
sensor = FullSensor(['position'], ['position', 'size'], variance=0.0)
env.add_sensor(sensor, [0])
env.set_reward_calculator(SampleReward())
env.set_spaces()
defaultAction = np.array([ 0.16851664, -0.38915175, -0.09081227])
pos0 = np.array([0.0, 0.0, 0.0])
vel0 = np.array([0.0, 0.0, 0.0])
for _ in range(100):
ob = env.reset(pos=pos0, vel=vel0)
print(f"Initial observation : {ob}")
#assert np.array_equal(initial_observations[0], ob)
# history = []
for _ in range(250):
action = defaultAction
ob, *_ = env.step(action)
Shouldn't robot respond to torque input? Is there something that I'm doing wrong? Everything seems fine in vel mode (and I even trained a simple RL agent with it) but I think there is something wrong with tor.