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