A visual-only robot affects the dynamics of another robot

[tomjur]

when using the setCollisionFilterGroupMask api to disallow collisions between the real robot and the visual only robot, I see that the visual only robot affects the movement of the real robot.

Below is a minimal script showing this problem:

1. A simulation with two robots (in the same base position) is created, each has it's own starting configuration and goal configuration, both act for 100 timesteps and the states of the robot1 (the real one) are recorded.
2. The simulation is reset only with robot1, using robot1's previous start and goal (states are recorded).
3. And again step 2.
4. I verified that the recorded states from step 2 and 3 are equal (i.e. there are no bugs in resetting the simulation)
5. I see that the states recorded in step 1 are not the same as in step 2.

import time

import numpy as np
import pybullet as p
import pybullet_data

# setup env
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 0)

def reset_env():
    p.setTimeStep(1.0 / 500)
    p.resetSimulation()
    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    p.setGravity(0, 0, -9.81)

reset_env()

# load both robots
def load_robot():
    return p.loadURDF(
        fileName="franka_panda/panda.urdf",
        basePosition=np.zeros(3), useFixedBase=True,
        flags=p.URDF_USE_SELF_COLLISION,
    )

robot1 = load_robot()
robot2 = load_robot()

# change dynamics (color and collision properties)
num_joints = p.getNumJoints(robot2)
color = [0.828125, 0.68359375, 0.21484375, 0.5] # second robot is gold
for link_id in range(num_joints):
    p.changeVisualShape(robot2, link_id, rgbaColor=color)
    # p.changeDynamics(robot2, link_id, mass=0.)
    p.setCollisionFilterGroupMask(robot1, link_id, robot1, robot2)
    p.setCollisionFilterGroupMask(robot2, link_id, 0, 0)

# get random start and goal configurations for both robots
joint_indices = np.array([0, 1, 2, 3, 4, 5, 6, 9, 10])
lower_limits, higher_limits = zip(*[p.getJointInfo(robot1, i)[8:10] for i in joint_indices[:7]])
lower_limits = np.array(lower_limits)
higher_limits = np.array(higher_limits)
m = 0.01 * (higher_limits - lower_limits)

def get_random_config():
    configuration = np.random.uniform(lower_limits + m, higher_limits - m)
    configuration = np.concatenate((configuration, np.zeros(2)))
    return configuration

configuration1 = get_random_config()
configuration2 = get_random_config()
goal_1 = get_random_config()
goal_2 = get_random_config()

def reset_robot_joints(robot_id, joints_configuration):
    for joint_index, joint_value in zip(joint_indices, joints_configuration):
        p.resetJointState(bodyUniqueId=robot_id, jointIndex=joint_index, targetValue=joint_value)

reset_robot_joints(robot1, configuration1)
reset_robot_joints(robot2, configuration2)

def set_robot_control(robot_id, goal_configuration):
    p.setJointMotorControlArray(
        robot_id,
        jointIndices=joint_indices,
        controlMode=p.POSITION_CONTROL,
        targetPositions=goal_configuration,
        forces=np.array([87.0, 87.0, 87.0, 87.0, 12.0, 120.0, 120.0, 170.0, 170.0]),
    )

set_robot_control(robot1, goal_1)
set_robot_control(robot2, goal_2)

def get_joints(robot_id):
    return [p.getJointState(robot_id, joint_index)[0] for joint_index in joint_indices]

two_robots_states = get_joints(robot1)
for i in range(100):
    time.sleep(0.01)
    p.stepSimulation()
    two_robots_states.append(get_joints(robot1))

p.resetSimulation()
reset_env()
robot1 = load_robot()
reset_robot_joints(robot1, configuration1)
set_robot_control(robot1, goal_1)

one_robot_states1 = get_joints(robot1)
for i in range(100):
    time.sleep(0.01)
    p.stepSimulation()
    one_robot_states1.append(get_joints(robot1))

p.resetSimulation()
reset_env()
robot1 = load_robot()
reset_robot_joints(robot1, configuration1)
set_robot_control(robot1, goal_1)

one_robot_states2 = get_joints(robot1)
for i in range(100):
    time.sleep(0.01)
    p.stepSimulation()
    one_robot_states2.append(get_joints(robot1))

# first assert that a single robot acts the same
assert len(one_robot_states1) == len(one_robot_states2)
for _, (one_robot_state1, one_robot_state2) in enumerate(zip(one_robot_states1, one_robot_states2)):
    assert np.array_equal(one_robot_state1, one_robot_state2)
print('single robot reset is OK')

# next, assert that with two robots there is a problem
assert len(one_robot_states1) == len(two_robots_states)
for _, (one_robot_state, two_robots_state) in enumerate(zip(one_robot_states1, two_robots_states)):
    assert np.array_equal(one_robot_state, two_robots_state)
print('two robots are consistent')

[erwincoumans]

Some determinism issue (small differences) are expected, since the visual robot is still added to the simulation (even with collisions turned off). You can add the visual robot to a different simulation if needed?

[tomjur]

Hi @erwincoumans, Thanks for the answer. Unfortunately, adding a visualization robot to a different simulation will not work.

I want to visualize the difference between the goal pose of the robot and a pre-collected trajectory (collected by a demonstration or a learned agent), by repeating the actions in the trajectory while keeping the goal fixed. Therefore I need the robot to visit the exact same states.

Do you think it will somehow be possible? Any alternative solution that visualizes the original trajectory alongside the goal would be great.

Thanks, Tom