Correct implementation of a visual-only robot

Official Python bindings with a focus on reinforcement learning and robotics.
Post Reply
tomjur
Posts: 2
Joined: Tue Oct 15, 2019 6:56 am

Correct implementation of a visual-only robot

Post by tomjur »

Hi all,

I want to have a visual only franka panda robot.
However, when I used 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. I create a simulation with two robots (in the same base position) 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.
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.

Any help would be appreciated, below is code I used for the above logic.
Thanks,
Tom

Code: Select all

import time

import np as np
import numpy as np
import pybullet as p
import pybullet_data


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()

robot1 = p.loadURDF(
    fileName="franka_panda/panda.urdf",
    basePosition=np.zeros(3), useFixedBase=True,
    flags=p.URDF_USE_SELF_COLLISION,
)


robot2 = p.loadURDF(
    fileName="franka_panda/panda.urdf",
    basePosition=np.zeros(3), useFixedBase=True,
    flags=p.URDF_USE_SELF_COLLISION,
)

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)

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()

for joint, angle1, angle2 in zip(joint_indices, configuration1, configuration2):
    p.resetJointState(bodyUniqueId=robot1, jointIndex=joint, targetValue=angle1)
    p.resetJointState(bodyUniqueId=robot2, jointIndex=joint, targetValue=angle2)

joint_forces = np.array([87.0, 87.0, 87.0, 87.0, 12.0, 120.0, 120.0, 170.0, 170.0])
p.setJointMotorControlArray(
    robot1,
    jointIndices=joint_indices,
    controlMode=p.POSITION_CONTROL,
    targetPositions=goal_1,
    forces=joint_forces,
)
p.setJointMotorControlArray(
    robot2,
    jointIndices=joint_indices,
    controlMode=p.POSITION_CONTROL,
    targetPositions=goal_2,
    forces=joint_forces,
)

def get_joints(robot_id):
    return [p.getJointState(robot_id, joint)[0] for joint 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 = p.loadURDF(
    fileName="franka_panda/panda.urdf",
    basePosition=np.zeros(3), useFixedBase=True,
    flags=p.URDF_USE_SELF_COLLISION,
)

for joint, angle1 in zip(joint_indices, configuration1):
    p.resetJointState(bodyUniqueId=robot1, jointIndex=joint, targetValue=angle1)

p.setJointMotorControlArray(
    robot1,
    jointIndices=joint_indices,
    controlMode=p.POSITION_CONTROL,
    targetPositions=goal_1,
    forces=joint_forces,
)

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 = p.loadURDF(
    fileName="franka_panda/panda.urdf",
    basePosition=np.zeros(3), useFixedBase=True,
    flags=p.URDF_USE_SELF_COLLISION,
)

for joint, angle1 in zip(joint_indices, configuration1):
    p.resetJointState(bodyUniqueId=robot1, jointIndex=joint, targetValue=angle1)

p.setJointMotorControlArray(
    robot1,
    jointIndices=joint_indices,
    controlMode=p.POSITION_CONTROL,
    targetPositions=goal_1,
    forces=joint_forces,
)

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')
Post Reply