def reset(self):
# reset simulation
del self.platform
self.platform = TriFingerPlatform(
visualization=self.visualization,
initial_robot_position=TriFingerPlatform.spaces.robot_position.
default,
initial_object_pose=self.initializer.get_initial_state(),
)
goal = self.initializer.get_goal()
self.goal = {
"position": goal.position,
"orientation": goal.orientation,
}
# visualize the goal
if self.visualization:
self.goal_marker = visual_objects.CubeMarker(
width=0.065,
position=goal.position,
orientation=goal.orientation,
)
self.info = {"difficulty": self.initializer.difficulty}
self.step_count = 0
return self._create_observation(0)
def reset(self):
# reset simulation
del self.platform
# initialize simulation
initial_robot_position = (
TriFingerPlatform.spaces.robot_position.default)
initial_object_pose = self.initializer.get_initial_state()
goal_object_pose = self.initializer.get_goal()
self.platform = TriFingerPlatform(
visualization=self.visualization,
initial_robot_position=initial_robot_position,
initial_object_pose=initial_object_pose,
)
self.goal = {
"position": goal_object_pose.position,
"orientation": goal_object_pose.orientation,
}
# visualize the goal
if self.visualization:
self.goal_marker = visual_objects.CubeMarker(
width=0.065,
position=goal_object_pose.position,
orientation=goal_object_pose.orientation,
physicsClientId=self.platform.simfinger._pybullet_client_id,
)
self.info = {"difficulty": self.initializer.difficulty}
self.step_count = 0
return self._create_observation(0)
def reset(self):
"""Reset the environment."""
# hard-reset simulation
del self.platform
# initialize simulation
initial_robot_position = (
TriFingerPlatform.spaces.robot_position.default)
# initialize cube at the centre
initial_object_pose = mct.move_cube.Pose(
position=mct.INITIAL_CUBE_POSITION)
self.platform = TriFingerPlatform(
visualization=self.visualization,
initial_robot_position=initial_robot_position,
initial_object_pose=initial_object_pose,
)
# get goal trajectory from the initializer
trajectory = self.initializer.get_trajectory()
# visualize the goal
if self.visualization:
self.goal_marker = visual_objects.CubeMarker(
width=mct.move_cube._CUBE_WIDTH,
position=trajectory[0][1],
orientation=(0, 0, 0, 1),
pybullet_client_id=self.platform.simfinger._pybullet_client_id,
)
self.info = {"time_index": -1, "trajectory": trajectory}
self.step_count = 0
return self._create_observation(0)
def reset(self):
# reset simulation
del self.platform
# initialize simulation
if self.initializer is None:
# if no initializer is given (which will be the case during training),
# we can initialize in any way desired. here, we initialize the cube always
# in the center of the arena, instead of randomly, as this appears to help
# training
initial_robot_position = (
TriFingerPlatform.spaces.robot_position.default
)
default_object_position = (
TriFingerPlatform.spaces.object_position.default
)
default_object_orientation = (
TriFingerPlatform.spaces.object_orientation.default
)
initial_object_pose = move_cube.Pose(
position=default_object_position,
orientation=default_object_orientation,
)
goal_object_pose = move_cube.sample_goal(difficulty=1)
else:
# if an initializer is given, i.e. during evaluation, we need to initialize
# according to it, to make sure we remain coherent with the standard CubeEnv.
# otherwise the trajectories produced during evaluation will be invalid.
initial_robot_position = (
TriFingerPlatform.spaces.robot_position.default
)
initial_object_pose = self.initializer.get_initial_state()
goal_object_pose = self.initializer.get_goal()
self.platform = TriFingerPlatform(
visualization=self.visualization,
initial_robot_position=initial_robot_position,
initial_object_pose=initial_object_pose,
)
self.goal = {
"position": goal_object_pose.position,
"orientation": goal_object_pose.orientation,
}
# visualize the goal
if self.visualization:
self.goal_marker = visual_objects.CubeMarker(
width=0.065,
position=goal_object_pose.position,
orientation=goal_object_pose.orientation,
pybullet_client_id=self.platform.simfinger._pybullet_client_id,
)
self.info = dict()
self.step_count = 0
return self._create_observation(0)
def main(logdir, video_path):
goal, difficulty = get_goal(logdir)
data = get_synced_log_data(logdir, goal, difficulty)
fps = len(data['t']) / (data['stamp'][-1] - data['stamp'][0])
video_recorder = VideoRecorder(fps)
cube_drawer = CubeDrawer(logdir)
initial_object_pose = move_cube.Pose(data['cube'][0].position,
data['cube'][0].orientation)
platform = trifinger_simulation.TriFingerPlatform(
visualization=True,
initial_object_pose=initial_object_pose,
)
markers = []
marker_cube_ori = VisualCubeOrientation(data['cube'][0].position,
data['cube'][0].orientation)
marker_goal_ori = VisualCubeOrientation(goal['position'], goal['orientation'])
visual_objects.CubeMarker(
width=0.065,
position=goal['position'],
orientation=goal['orientation']
)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=0, cameraPitch=-40, cameraTargetPosition=[0,0,0])
for i, t in enumerate(data['t']):
platform.simfinger.reset_finger_positions_and_velocities(data['desired_action'][i].position)
platform.cube.set_state(data['cube'][i].position, data['cube'][i].orientation)
marker_cube_ori.set_state(data['cube'][i].position, data['cube'][i].orientation)
frame_desired = video_recorder.get_views()
frame_desired = cv2.cvtColor(frame_desired, cv2.COLOR_RGB2BGR)
platform.simfinger.reset_finger_positions_and_velocities(data['robot'][i].position)
frame_observed = video_recorder.get_views()
frame_observed = cv2.cvtColor(frame_observed, cv2.COLOR_RGB2BGR)
frame_real = np.concatenate(data['images'][i], axis=1)
frame_real_cube = np.concatenate(cube_drawer.add_cube(data['images'][i],
data['cube'][i]),
axis=1)
frame = vstack_frames((frame_desired, frame_observed, frame_real, frame_real_cube))
# frame = np.concatenate((frame_desired, frame_observed,
# frame_real, frame_real_cube), axis=0)
# add text
frame = add_text(frame, text="step: {:06d}".format(t), position=(10, 40))
frame = add_text(frame, text="acc reward: {:.3f}".format(data["acc_reward"][i]), position=(10, 70))
frame = add_text(
frame,
text="tip force {}".format(
np.array2string(data["robot"][i].tip_force, precision=3),
),
position=(10, 100),
)
video_recorder.add_frame(frame)
video_recorder.save_video(video_path)
def visualize_collisions(sim):
contact_points = pybullet.getContactPoints(
bodyA=sim.finger_id,
physicsClientId=sim._pybullet_client_id,
)
markers = []
for cp in contact_points:
contact_distance = cp[8]
if contact_distance < 0:
position = cp[5]
marker = visual_objects.CubeMarker(
width=0.01,
position=position,
orientation=(0, 0, 0, 1),
color=(1, 0, 0, 1),
)
markers.append(marker)
return markers