scene doesn't seem to visually update after setting joint positions directly

[tomsilver]

In the code below, I am attempting to directly change the robot joint positions. It appears that the data is updated correctly. But when I use --graphics, I don't see any change in the robot's position after setting the joints. Based on playing with the joint GUI, it seems like the change should noticeably rotate the robot counterclockwise.

from pyrcareworld.envs.base_env import RCareWorld
import numpy as np
import os
import argparse

def _main(use_graphics=False):

    script_dir = os.path.dirname(os.path.abspath(__file__))
    executable_file = os.path.join(script_dir, "Bathing", "BathingPlayer.x86_64")
    env = RCareWorld(executable_file=executable_file, graphics=use_graphics)

    stretch_id = 221582
    robot = env.GetAttr(stretch_id)
    robot.EnabledNativeIK(False)

    current_joints = robot.data["joint_positions"]
    next_joints = list(current_joints)
    next_joints[0] = current_joints[0] - 100  # should be feasible according to env.ShowArticulationParameter(robot.id)
    robot.SetJointPositionDirectly(next_joints)
    env.step(300)

    recovered_joints = robot.data["joint_positions"]
    print(recovered_joints)  # looks correct

    # But the simulation visually doesn't update!


if __name__ == "__main__":
    parser = argparse.ArgumentParser(description='Run RCareWorld bathing environment simulation.')
    parser.add_argument('-g', '--graphics', action='store_true', help='Enable graphics')
    args = parser.parse_args()
    _main(use_graphics=args.graphics)

[YoruCathy]

Hi @tomsilver , you need to step the environment once after setting robot.EnabledNativeIK to False. The updated code is like the following:

from pyrcareworld.envs.base_env import RCareWorld
import numpy as np
import os
import argparse

def _main(use_graphics=False):

    script_dir = os.path.dirname(os.path.abspath(__file__))
    executable_file = os.path.join(script_dir, "Bathing", "BathingPlayer.x86_64")
    env = RCareWorld(executable_file=executable_file, graphics=use_graphics)

    stretch_id = 221582
    robot = env.GetAttr(stretch_id)
    robot.EnabledNativeIK(False)
    env.step()

    print(robot.data)

    current_joints = robot.data["joint_positions"]
    next_joints = list(current_joints)
    # next_joints[5] = current_joints[5] - 100  # should be feasible according to env.ShowArticulationParameter(robot.id)
    next_joints = list(np.random.uniform(-180, 180, len(current_joints)))
    robot.SetJointPositionDirectly(next_joints)
    env.step()
    env.Pend()

    recovered_joints = robot.data["joint_positions"]
    print(recovered_joints)  # looks correct
    env.Pend()



if __name__ == "__main__":
    parser = argparse.ArgumentParser(description='Run RCareWorld bathing environment simulation.')
    parser.add_argument('-g', '--graphics', action='store_true', help='Enable graphics')
    args = parser.parse_args()
    _main(use_graphics=args.graphics)

[tomsilver]

Thanks for the help! Do you have any idea why this line in my original snippet doesn't seem to have an effect, even with the change you suggested?

next_joints[0] = current_joints[0] - 100  # should be feasible according to env.ShowArticulationParameter(robot.id)

[YoruCathy]

Maybe this is not in the joint limit? Check the joint limits in robot.data

[tomsilver]

Resolved in person: the joint limits were not the issue -- instead, it turns out that the robot base can only be changed through the Turn and Move APIs, rather than setting the joints directly.