Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 1 addition & 2 deletions demos/demo_trifinger_platform.py
Original file line number Diff line number Diff line change
Expand Up @@ -76,8 +76,7 @@ def main():
if args.object == "cube":
camera_observation = platform.get_camera_observation(t)
print(
"Cube Position: %s"
% camera_observation.filtered_object_pose.position
"Cube Position: %s" % camera_observation.filtered_object_pose.position
)

if platform.enable_cameras:
Expand Down
4 changes: 1 addition & 3 deletions docs/sim_finger_screenshots.py
Original file line number Diff line number Diff line change
Expand Up @@ -76,9 +76,7 @@
cube = collision_objects.ColoredCubeV2(
position=config["cube_pos"],
# rotate the cube a bit, so it looks nicer
orientation=Rotation.from_euler(
"xz", (90, -40), degrees=True
).as_quat(),
orientation=Rotation.from_euler("xz", (90, -40), degrees=True).as_quat(),
)

# set the finger position
Expand Down
3 changes: 0 additions & 3 deletions pyproject.toml
Original file line number Diff line number Diff line change
@@ -1,9 +1,6 @@
[build-system]
requires = ["setuptools>=42", "wheel"]

[tool.black]
line-length = 79

[tool.ruff]
lint.select = [
"E",
Expand Down
8 changes: 2 additions & 6 deletions scripts/playback_data.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,7 @@ def main_finger(finger_type, data_file):

def reset_finger(joint_position):
for i, joint_id in enumerate(finger.pybullet_joint_indices):
pybullet.resetJointState(
finger.finger_id, joint_id, joint_position[i]
)
pybullet.resetJointState(finger.finger_id, joint_id, joint_position[i])

with open(data_file, "rb") as file_handle:
episodes = pickle.load(file_handle)
Expand Down Expand Up @@ -93,9 +91,7 @@ def main_trifinger(finger_type, data_file_0, data_file_120, data_file_240):

def reset_finger(joint_position):
for i, joint_id in enumerate(finger.pybullet_joint_indices):
pybullet.resetJointState(
finger.finger_id, joint_id, joint_position[i]
)
pybullet.resetJointState(finger.finger_id, joint_id, joint_position[i])

data = []
for filename in (data_file_0, data_file_120, data_file_240):
Expand Down
4 changes: 1 addition & 3 deletions scripts/plot_tip_position_trajectories.py
Original file line number Diff line number Diff line change
Expand Up @@ -105,9 +105,7 @@ def main():
trajectory = np.vstack(data["tip_positions"])
dist = trajectory - tip_goal

ax[a].plot(
dist[:, x], dist[:, y], color=(0.3, 0.3, 0.3), linewidth=2
)
ax[a].plot(dist[:, x], dist[:, y], color=(0.3, 0.3, 0.3), linewidth=2)
ax[a].plot(dist[0, x], dist[0, y], "bo", zorder=40)
ax[a].plot(dist[-1, x], dist[-1, y], "g^", zorder=40)

Expand Down
4 changes: 1 addition & 3 deletions scripts/rrc_evaluate.py
Original file line number Diff line number Diff line change
Expand Up @@ -50,9 +50,7 @@ def main():
# some basic checks of the input
if not args.singularity_image.exists():
print(
"The singularity image {} does not exist.".format(
args.singularity_image
),
"The singularity image {} does not exist.".format(args.singularity_image),
file=sys.stderr,
)
return 1
Expand Down
4 changes: 1 addition & 3 deletions scripts/screenshot_mode.py
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,7 @@ def main():
parser.add_argument(
"finger_type",
choices=valid_finger_types,
help="Specify a valid finger type. One of {}".format(
valid_finger_types
),
help="Specify a valid finger type. One of {}".format(valid_finger_types),
)
parser.add_argument(
"--position", "-p", type=float, nargs="+", help="Joint positions."
Expand Down
38 changes: 10 additions & 28 deletions scripts/view_camera_log_real_and_sim.py
Original file line number Diff line number Diff line change
Expand Up @@ -20,12 +20,8 @@

def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"log_dir", type=pathlib.Path, help="Path to the log files."
)
parser.add_argument(
"--rate", type=int, default=1, help="Time in ms per step."
)
parser.add_argument("log_dir", type=pathlib.Path, help="Path to the log files.")
parser.add_argument("--rate", type=int, default=1, help="Time in ms per step.")
args = parser.parse_args()

robot_log_file = str(args.log_dir / "robot_data.dat")
Expand All @@ -40,8 +36,7 @@ def main():
cameras = camera.create_trifinger_camera_array_from_config(args.log_dir)

cube_urdf_file = (
pathlib.Path(trifinger_simulation.__file__).parent
/ "data/cube_v2/cube_v2.urdf"
pathlib.Path(trifinger_simulation.__file__).parent / "data/cube_v2/cube_v2.urdf"
)
cube = pybullet.loadURDF(
fileName=str(cube_urdf_file),
Expand All @@ -50,31 +45,19 @@ def main():

pybullet.configureDebugVisualizer(lightPosition=(0, 0, 1.0))
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SHADOWS, 1)
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0
)
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0
)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
# yes, it is really a segmentation maRk...
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0
)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)

for t in range(
log.get_first_timeindex(), log.get_last_timeindex() + 1, 100
):
for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1, 100):
robot_observation = log.get_robot_observation(t)
simulation.reset_finger_positions_and_velocities(
robot_observation.position
)
simulation.reset_finger_positions_and_velocities(robot_observation.position)

# get rendered images from simulation
sim_images = cameras.get_images()
# images are rgb --> convert to bgr for opencv
sim_images = [
cv2.cvtColor(img, cv2.COLOR_RGB2BGR) for img in sim_images
]
sim_images = [cv2.cvtColor(img, cv2.COLOR_RGB2BGR) for img in sim_images]
sim_images = np.hstack(sim_images)

# get real images from cameras
Expand All @@ -88,8 +71,7 @@ def main():
)

real_images = [
utils.convert_image(cam.image)
for cam in camera_observation.cameras
utils.convert_image(cam.image) for cam in camera_observation.cameras
]
real_images = np.hstack(real_images)
except Exception as e:
Expand Down
28 changes: 7 additions & 21 deletions tests/test_camera.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,7 @@ def test_calib_data_to_matrix_1x1():
}
expected = np.array([[42]])

np.testing.assert_array_equal(
sim_camera.calib_data_to_matrix(data), expected
)
np.testing.assert_array_equal(sim_camera.calib_data_to_matrix(data), expected)


def test_calib_data_to_matrix_3x3():
Expand All @@ -32,9 +30,7 @@ def test_calib_data_to_matrix_3x3():
}
expected = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])

np.testing.assert_array_equal(
sim_camera.calib_data_to_matrix(data), expected
)
np.testing.assert_array_equal(sim_camera.calib_data_to_matrix(data), expected)


def test_calib_data_to_matrix_2x4():
Expand All @@ -45,9 +41,7 @@ def test_calib_data_to_matrix_2x4():
}
expected = np.array([[1, 2, 3, 4], [5, 6, 7, 8]])

np.testing.assert_array_equal(
sim_camera.calib_data_to_matrix(data), expected
)
np.testing.assert_array_equal(sim_camera.calib_data_to_matrix(data), expected)


def test_camera_parameters_load():
Expand All @@ -61,9 +55,7 @@ def test_camera_parameters_load():
]
)

expected_dist_coeffs = np.array(
[-0.2373, 0.1466, -0.0033, 0.0002, -0.1356]
)
expected_dist_coeffs = np.array([-0.2373, 0.1466, -0.0033, 0.0002, -0.1356])

expected_tf_mat = np.array(
[
Expand All @@ -81,15 +73,11 @@ def test_camera_parameters_load():
assert params.name == "camera180"
assert params.width == 720
assert params.height == 540
np.testing.assert_array_almost_equal(
params.camera_matrix, expected_camera_matrix
)
np.testing.assert_array_almost_equal(params.camera_matrix, expected_camera_matrix)
np.testing.assert_array_almost_equal(
params.distortion_coefficients, expected_dist_coeffs
)
np.testing.assert_array_almost_equal(
params.tf_world_to_camera, expected_tf_mat
)
np.testing.assert_array_almost_equal(params.tf_world_to_camera, expected_tf_mat)


def test_camera_parameters_dump(tmpdir):
Expand All @@ -110,9 +98,7 @@ def test_camera_parameters_dump(tmpdir):
assert params.name == params2.name
assert params.width == params2.width
assert params.height == params2.height
np.testing.assert_array_almost_equal(
params.camera_matrix, params2.camera_matrix
)
np.testing.assert_array_almost_equal(params.camera_matrix, params2.camera_matrix)
np.testing.assert_array_almost_equal(
params.distortion_coefficients, params2.distortion_coefficients
)
Expand Down
4 changes: 1 addition & 3 deletions tests/test_finger_types_data.py
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,7 @@ def test_check_finger_type():

def test_get_finger_urdf():
for name in ftd.finger_types_data.keys():
assert (
ftd.get_finger_urdf(name) == ftd.finger_types_data[name].urdf_file
)
assert ftd.get_finger_urdf(name) == ftd.finger_types_data[name].urdf_file

with pytest.raises(ValueError):
ftd.check_finger_type("invalid")
Expand Down
4 changes: 1 addition & 3 deletions tests/test_loading_urdfs.py
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,7 @@ def test_loading_urdfs(self):

except pybullet.error as e:
self.fail(
"Failed to create SimFinger(finger_type={}): {}".format(
key, e
)
"Failed to create SimFinger(finger_type={}): {}".format(key, e)
)

def test_loading_urdfs_locally(self):
Expand Down
4 changes: 1 addition & 3 deletions tests/test_reset_joints.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,9 +25,7 @@ def test_reproduce_reset_state(self):
)

for _ in range(100):
state_positions = sample.random_joint_positions(
finger.number_of_fingers
)
state_positions = sample.random_joint_positions(finger.number_of_fingers)
state_velocities = [pos * 10 for pos in state_positions]

reset_state = finger.reset_finger_positions_and_velocities(
Expand Down
8 changes: 2 additions & 6 deletions tests/test_sample.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,17 +30,13 @@ def test_random_point_positions(self):

# one finger
for i in range(100):
result = sample.random_joint_positions(
1, lower_bounds, upper_bounds
)
result = sample.random_joint_positions(1, lower_bounds, upper_bounds)
assert_array_less_equal(lower_bounds, result)
assert_array_less_equal(result, upper_bounds)

# three finger
for i in range(100):
result = sample.random_joint_positions(
3, lower_bounds, upper_bounds
)
result = sample.random_joint_positions(3, lower_bounds, upper_bounds)
assert_array_less_equal(lower_bounds * 3, result)
assert_array_less_equal(result, upper_bounds * 3)

Expand Down
28 changes: 7 additions & 21 deletions tests/test_sim_finger.py
Original file line number Diff line number Diff line change
Expand Up @@ -20,41 +20,27 @@ def test_SimFinger_sanitise_torqes():
max_torque = 0.5

# without velocity damping
t = SimFinger._sanitise_torques(
[-0.2, 0.4], max_torque, 0, np.array([0, 0])
)
t = SimFinger._sanitise_torques([-0.2, 0.4], max_torque, 0, np.array([0, 0]))
np.testing.assert_array_equal(t, [-0.2, 0.4])

t = SimFinger._sanitise_torques(
[-1.2, 0.4], max_torque, 0, np.array([0, 0])
)
t = SimFinger._sanitise_torques([-1.2, 0.4], max_torque, 0, np.array([0, 0]))
np.testing.assert_array_equal(t, [-0.5, 0.4])

t = SimFinger._sanitise_torques(
[-0.2, 0.6], max_torque, 0, np.array([0, 0])
)
t = SimFinger._sanitise_torques([-0.2, 0.6], max_torque, 0, np.array([0, 0]))
np.testing.assert_array_equal(t, [-0.2, 0.5])

t = SimFinger._sanitise_torques(
[-0.8, 0.6], max_torque, 0, np.array([0, 0])
)
t = SimFinger._sanitise_torques([-0.8, 0.6], max_torque, 0, np.array([0, 0]))
np.testing.assert_array_equal(t, [-0.5, 0.5])

# with velocity damping
t = SimFinger._sanitise_torques(
[0, 0], max_torque, 0.2, np.array([1, -0.5])
)
t = SimFinger._sanitise_torques([0, 0], max_torque, 0.2, np.array([1, -0.5]))
np.testing.assert_array_almost_equal(t, [-0.2, 0.1])

t = SimFinger._sanitise_torques([0, 0], max_torque, 1, np.array([1, -0.6]))
np.testing.assert_array_almost_equal(t, [-0.5, 0.5])

t = SimFinger._sanitise_torques(
[0.1, 0.2], max_torque, 0.2, np.array([1, -0.5])
)
t = SimFinger._sanitise_torques([0.1, 0.2], max_torque, 0.2, np.array([1, -0.5]))
np.testing.assert_array_almost_equal(t, [-0.1, 0.3])

t = SimFinger._sanitise_torques(
[1, -1], max_torque, 0.2, np.array([1, -0.5])
)
t = SimFinger._sanitise_torques([1, -1], max_torque, 0.2, np.array([1, -0.5]))
np.testing.assert_array_almost_equal(t, [0.3, -0.4])
Loading
Loading