def evaluate_state( trajectory: Trajectory, time_index: int, actual_position: Position ): """Compute cost of a given cube pose at a given time step. Less is better. The cost is computed using :func:`move_cube.evaluate_state` (difficulty=3) for the goal position that is active at the given time_index. Args: trajectory: The trajectory based on which the cost is computed. time_index: Index of the time step that is evaluated. actual_position: Cube position at the specified time step. Returns: Cost of the actual position w.r.t. to the goal position of the active step in the trajectory. Lower value means that the actual pose is closer to the goal. Zero if actual == goal. """ active_goal = get_active_goal(trajectory, time_index) # wrap positions in Pose objects actual_pose = move_cube.Pose(position=actual_position) goal_pose = move_cube.Pose(position=active_goal) return move_cube.evaluate_state(goal_pose, actual_pose, GOAL_DIFFICULTY)
def test_evaluate_state_difficulty_4_rotation_around_y(self): difficulty = 4 pose_origin = move_cube.Pose() pose_rot_only_y = move_cube.Pose( orientation=Rotation.from_euler("y", 0.42).as_quat() ) pose_rot_without_y = move_cube.Pose( orientation=Rotation.from_euler("yz", [0.0, 0.42]).as_quat() ) pose_rot_with_y = move_cube.Pose( orientation=Rotation.from_euler("yz", [0.2, 0.42]).as_quat() ) # needs to be zero for exact match cost = move_cube.evaluate_state(pose_origin, pose_origin, difficulty) self.assertEqual(cost, 0) cost_only_y = move_cube.evaluate_state( pose_origin, pose_rot_only_y, difficulty ) self.assertEqual(cost_only_y, 0) cost_without_y = move_cube.evaluate_state( pose_origin, pose_rot_without_y, difficulty ) cost_with_y = move_cube.evaluate_state( pose_origin, pose_rot_with_y, difficulty ) self.assertAlmostEqual(cost_without_y, cost_with_y)
def test_evaluate_state_difficulty_4(self): difficulty = 4 pose_origin = move_cube.Pose() pose_trans = move_cube.Pose(position=[1, 2, 3]) pose_rot = move_cube.Pose( orientation=Rotation.from_euler("z", 0.42).as_quat() ) pose_both = move_cube.Pose( [1, 2, 3], Rotation.from_euler("z", 0.42).as_quat() ) # needs to be zero for exact match cost = move_cube.evaluate_state(pose_origin, pose_origin, difficulty) self.assertEqual(cost, 0) # None-zero if there is translation, rotation or both self.assertNotEqual( move_cube.evaluate_state(pose_origin, pose_trans, difficulty), 0 ) self.assertNotEqual( move_cube.evaluate_state(pose_origin, pose_rot, difficulty), 0 ) self.assertNotEqual( move_cube.evaluate_state(pose_origin, pose_both, difficulty), 0 )
def get_synced_log_data(logdir, goal, difficulty): log = robot_fingers.TriFingerPlatformLog(os.path.join(logdir, "robot_data.dat"), os.path.join(logdir, "camera_data.dat")) log_camera = tricamera.LogReader(os.path.join(logdir, "camera_data.dat")) stamps = log_camera.timestamps obs = {'robot': [], 'cube': [], 'images': [], 't': [], 'desired_action': [], 'stamp': [], 'acc_reward': []} ind = 0 acc_reward = 0.0 for t in range(log.get_first_timeindex(), log.get_last_timeindex()): camera_observation = log.get_camera_observation(t) acc_reward -= move_cube.evaluate_state( move_cube.Pose(**goal), camera_observation.filtered_object_pose, difficulty ) if 1000 * log.get_timestamp_ms(t) >= stamps[ind]: robot_observation = log.get_robot_observation(t) obs['robot'].append(robot_observation) obs['cube'].append(camera_observation.filtered_object_pose) obs['images'].append([convert_image(camera.image) for camera in camera_observation.cameras]) obs['desired_action'].append(log.get_desired_action(t)) obs['acc_reward'].append(acc_reward) obs['t'].append(t) obs['stamp'].append(log.get_timestamp_ms(t)) ind += 1 return obs
def compute_reward(logdir, simulation): with open(os.path.join(logdir, "goal.json"), 'r') as f: goal = json.load(f) difficulty = goal['difficulty'] goal_pose = move_cube.Pose(position=np.array(goal['goal']['position']), orientation=np.array( goal['goal']['orientation'])) if (simulation == 0): log = robot_fingers.TriFingerPlatformLog( os.path.join(logdir, "robot_data.dat"), os.path.join(logdir, "camera_data.dat")) reward = 0.0 for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1): camera_observation = log.get_camera_observation(t) reward -= evaluate_state(goal_pose, camera_observation.filtered_object_pose, difficulty) return reward, True else: path = os.path.join(logdir, 'observations.pkl') with open(path, 'rb') as handle: observations = pkl.load(handle) reward = 0.0 ex_state = move_cube.sample_goal(difficulty=-1) for i in range(len(observations)): ex_state.position = observations[i]["achieved_goal"]["position"] ex_state.orientation = observations[i]["achieved_goal"][ "orientation"] reward -= evaluate_state(goal_pose, ex_state, difficulty) return reward, True
def test_evaluate_state_dict(): """Test evaluate state using a dict instead of Pose.""" difficulty = 4 pose_origin = move_cube.Pose() dict_origin = {"position": [0, 0, 0], "orientation": [0, 0, 0, 1]} pose_pose = move_cube.Pose([1, 2, 3], Rotation.from_euler("z", 0.42).as_quat()) dict_pose = { "position": [1, 2, 3], "orientation": Rotation.from_euler("z", 0.42).as_quat(), } # needs to be zero for exact match pose_cost = move_cube.evaluate_state(pose_origin, pose_pose, difficulty) dict_cost = move_cube.evaluate_state(dict_origin, dict_pose, difficulty) assert pose_cost == dict_cost
def test_evaluate_state_difficulty_3(): difficulty = 3 pose_origin = move_cube.Pose() pose_trans = move_cube.Pose(position=[1, 2, 3]) pose_rot = move_cube.Pose( orientation=Rotation.from_euler("z", 0.42).as_quat()) pose_both = move_cube.Pose([1, 2, 3], Rotation.from_euler("z", 0.42).as_quat()) # needs to be zero for exact match cost = move_cube.evaluate_state(pose_origin, pose_origin, difficulty) assert cost == 0 # None-zero if there is translation, rotation is ignored assert move_cube.evaluate_state(pose_origin, pose_trans, difficulty) != 0 assert move_cube.evaluate_state(pose_origin, pose_rot, difficulty) == 0 assert move_cube.evaluate_state(pose_origin, pose_both, difficulty) != 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 compute_reward_adaptive_behind(logdir, simulation, TOTALTIMESTEPS): with open(os.path.join(logdir, "goal.json"), 'r') as f: goal = json.load(f) difficulty = goal['difficulty'] goal_pose = move_cube.Pose(position=np.array(goal['goal']['position']), orientation=np.array( goal['goal']['orientation'])) indice = goal['reachstart'] if (indice == -1): return -10000, False if (simulation == 0): min_length = TOTALTIMESTEPS log = robot_fingers.TriFingerPlatformLog( os.path.join(logdir, "robot_data.dat"), os.path.join(logdir, "camera_data.dat")) reward = 0.0 count = 0 for t in range(log.get_last_timeindex() - TOTALTIMESTEPS, log.get_last_timeindex() + 1): count += 1 camera_observation = log.get_camera_observation(t) reward -= evaluate_state(goal_pose, camera_observation.filtered_object_pose, difficulty) if (count == min_length): break if (count == min_length): return reward, True else: return -10000, False else: min_length = TOTALTIMESTEPS path = os.path.join(logdir, 'observations.pkl') with open(path, 'rb') as handle: observations = pkl.load(handle) reward = 0.0 count = 0 ex_state = move_cube.sample_goal(difficulty=-1) for i in range( len(observations) - TOTALTIMESTEPS - 1, len(observations)): count += 1 ex_state.position = observations[i]["achieved_goal"]["position"] ex_state.orientation = observations[i]["achieved_goal"][ "orientation"] reward -= evaluate_state(goal_pose, ex_state, difficulty) if (count == min_length): break if (count == min_length): return reward, True else: return -10000, False
def compute_reward(logdir): log = robot_fingers.TriFingerPlatformLog(os.path.join(logdir, "robot_data.dat"), os.path.join(logdir, "camera_data.dat")) with open(os.path.join(logdir, "goal.json"), 'r') as f: goal = json.load(f) difficulty = goal['difficulty'] goal_pose = move_cube.Pose(position=np.array(goal['goal']['position']), orientation=np.array(goal['goal']['orientation'])) reward = 0.0 for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1): camera_observation = log.get_camera_observation(t) reward -= move_cube.evaluate_state( goal_pose, camera_observation.filtered_object_pose, difficulty ) return reward
def validate_goal(trajectory: Trajectory): """Checks if the given trajectory is valid, raises error if not. Raises: ValueError: If there are any structural issues. move_cube.InvalidGoalError: If a position exceeds the allowed goal space. """ previous_t = -1 if not trajectory: raise ValueError("Trajectory is empty") if trajectory[0][0] != 0: raise ValueError("First goal does not start at t=0") for i, (t, goal) in enumerate(trajectory): if t <= previous_t: raise ValueError(f"Goal {i} starts before previous goal") previous_t = t move_cube.validate_goal(move_cube.Pose(position=goal))
def reset(self): # By changing the `_reset_*` method below you can switch between using # the platform frontend, which is needed for the submission system, and # the direct simulation, which may be more convenient if you want to # pre-train locally in simulation. self._reset_platform_frontend() if self.sim_platform: del self.sim_platform initial_robot_position = TriFingerPlatform.spaces.robot_position.default default_object_position = ( TriFingerPlatform.spaces.object_position.default) default_object_orientation = ( TriFingerPlatform.spaces.object_orientation.default) dummy_initial_object_pose = move_cube.Pose( position=default_object_position, orientation=default_object_orientation, ) self.sim_platform = TriFingerPlatform( visualization=False, initial_robot_position=initial_robot_position, initial_object_pose=dummy_initial_object_pose, ) # self.goal_marker = visual_objects.CubeMarker( # width=0.065, # position=self.goal["position"], # orientation=self.goal["orientation"], # physicsClientId=self.sim_platform.simfinger._pybullet_client_id, # ) # self._reset_direct_simulation() self.step_count = 0 # need to already do one step to get initial observation # TODO disable frameskip here? observation, _, _, _ = self.step(self._initial_action) return observation
def test_validate_goal(): half_width = move_cube._CUBE_WIDTH / 2 yaw_rotation = Rotation.from_euler("z", 0.42).as_quat() full_rotation = Rotation.from_euler("zxz", [0.42, 0.1, -2.3]).as_quat() # test some valid goals try: move_cube.validate_goal( move_cube.Pose([0, 0, half_width], [0, 0, 0, 1])) except Exception as e: pytest.fail("Valid goal was considered invalid because %s" % e) try: move_cube.validate_goal( move_cube.Pose([0.05, -0.1, half_width], yaw_rotation)) except Exception as e: pytest.fail("Valid goal was considered invalid because %s" % e) try: move_cube.validate_goal( move_cube.Pose([-0.12, 0.0, 0.06], full_rotation)) except Exception as e: pytest.fail("Valid goal was considered invalid because %s" % e) # test some invalid goals # invalid values with pytest.raises(ValueError): move_cube.validate_goal(move_cube.Pose([0, 0], [0, 0, 0, 1])) with pytest.raises(ValueError): move_cube.validate_goal(move_cube.Pose([0, 0, 0], [0, 0, 1])) # invalid positions with pytest.raises(move_cube.InvalidGoalError): move_cube.validate_goal( move_cube.Pose([0.3, 0, half_width], [0, 0, 0, 1])) with pytest.raises(move_cube.InvalidGoalError): move_cube.validate_goal( move_cube.Pose([0, -0.3, half_width], [0, 0, 0, 1])) with pytest.raises(move_cube.InvalidGoalError): move_cube.validate_goal(move_cube.Pose([0, 0, 0.3], [0, 0, 0, 1])) with pytest.raises(move_cube.InvalidGoalError): move_cube.validate_goal(move_cube.Pose([0, 0, 0], [0, 0, 0, 1])) with pytest.raises(move_cube.InvalidGoalError): move_cube.validate_goal(move_cube.Pose([0, 0, -0.01], [0, 0, 0, 1])) # valid CoM position but rotation makes it reach out of valid range with pytest.raises(move_cube.InvalidGoalError): move_cube.validate_goal( move_cube.Pose([0, 0, half_width], full_rotation))
class TriFingerPlatform: """ Wrapper around the simulation providing the same interface as ``robot_interfaces::TriFingerPlatformFrontend``. The following methods of the robot_interfaces counterpart are not supported: - get_robot_status() - wait_until_timeindex() """ # Create the action and observation spaces # ======================================== _n_joints = 9 _n_fingers = 3 _max_torque_Nm = 0.36 _max_velocity_radps = 10 spaces = SimpleNamespace() spaces.robot_torque = SimpleNamespace( low=np.full(_n_joints, -_max_torque_Nm, dtype=np.float32), high=np.full(_n_joints, _max_torque_Nm, dtype=np.float32), default=np.zeros(_n_joints, dtype=np.float32), ) spaces.robot_position = SimpleNamespace( low=np.array([-0.9, -1.57, -2.7] * _n_fingers, dtype=np.float32), high=np.array([1.4, 1.57, 0.0] * _n_fingers, dtype=np.float32), default=np.array( [0.0, np.deg2rad(70), np.deg2rad(-130)] * _n_fingers, dtype=np.float32, ), ) spaces.robot_velocity = SimpleNamespace( low=np.full(_n_joints, -_max_velocity_radps, dtype=np.float32), high=np.full(_n_joints, _max_velocity_radps, dtype=np.float32), default=np.zeros(_n_joints, dtype=np.float32), ) spaces.object_position = SimpleNamespace( low=np.array([-0.3, -0.3, 0], dtype=np.float32), high=np.array([0.3, 0.3, 0.3], dtype=np.float32), default=np.array([0, 0, move_cube._min_height], dtype=np.float32), ) spaces.object_orientation = SimpleNamespace( low=-np.ones(4, dtype=np.float32), high=np.ones(4, dtype=np.float32), default=move_cube.Pose().orientation, ) # for convenience, we also create the respective gym spaces spaces.robot_torque.gym = gym.spaces.Box(low=spaces.robot_torque.low, high=spaces.robot_torque.high) spaces.robot_position.gym = gym.spaces.Box(low=spaces.robot_position.low, high=spaces.robot_position.high) spaces.robot_velocity.gym = gym.spaces.Box(low=spaces.robot_velocity.low, high=spaces.robot_velocity.high) spaces.object_position.gym = gym.spaces.Box( low=spaces.object_position.low, high=spaces.object_position.high) spaces.object_orientation.gym = gym.spaces.Box( low=spaces.object_orientation.low, high=spaces.object_orientation.high) def __init__( self, visualization=False, initial_robot_position=None, initial_object_pose=None, enable_cameras=False, ): """Initialize. Args: visualization (bool): Set to true to run visualization. initial_robot_position: Initial robot joint angles initial_object_pose: Initial pose for the manipulation object. Can be any object with attributes ``position`` (x, y, z) and ``orientation`` (x, y, z, w). This is optional, if not set, a random pose will be sampled. enable_cameras (bool): Set to true to enable camera observations. By default this is disabled as rendering of images takes a lot of computational power. Therefore the cameras should only be enabled if the images are actually used. """ #: Camera rate in frames per second. Observations of camera and #: object pose will only be updated with this rate. #: NOTE: This is currently not used! self._camera_rate_fps = 30 #: Set to true to render camera observations self.enable_cameras = enable_cameras #: Simulation time step self._time_step = 0.004 # first camera update in the first step self._next_camera_update_step = 0 # Initialize robot, object and cameras # ==================================== self.simfinger = SimFinger( finger_type="trifingerpro", time_step=self._time_step, enable_visualization=visualization, ) if initial_robot_position is None: initial_robot_position = self.spaces.robot_position.default self.simfinger.reset_finger_positions_and_velocities( initial_robot_position) if initial_object_pose is None: initial_object_pose = move_cube.Pose( position=self.spaces.object_position.default, orientation=self.spaces.object_orientation.default, ) self.cube = collision_objects.Block( initial_object_pose.position, initial_object_pose.orientation, mass=0.020, ) self.tricamera = camera.TriFingerCameras() # Forward some methods for convenience # ==================================== # forward "RobotFrontend" methods directly to simfinger self.Action = self.simfinger.Action self.get_desired_action = self.simfinger.get_desired_action self.get_applied_action = self.simfinger.get_applied_action self.get_timestamp_ms = self.simfinger.get_timestamp_ms self.get_current_timeindex = self.simfinger.get_current_timeindex self.get_robot_observation = self.simfinger.get_observation # forward kinematics directly to simfinger self.forward_kinematics = ( self.simfinger.pinocchio_utils.forward_kinematics) # Initialize log # ============== self._action_log = { "initial_robot_position": initial_robot_position.tolist(), "initial_object_pose": { "position": initial_object_pose.position.tolist(), "orientation": initial_object_pose.orientation.tolist(), }, "actions": [], } def get_time_step(self): """Get simulation time step in seconds.""" return self._time_step def _compute_camera_update_step_interval(self): return (1.0 / self._camera_rate_fps) / self._time_step def append_desired_action(self, action): """ Call :meth:`pybullet.SimFinger.append_desired_action` and add the action to the action log. Arguments/return value are the same as for :meth:`pybullet.SimFinger.append_desired_action`. """ # update camera and object observations only with the rate of the # cameras # next_t = self.get_current_timeindex() + 1 # has_camera_update = next_t >= self._next_camera_update_step # if has_camera_update: # self._next_camera_update_step += ( # self._compute_camera_update_step_interval() # ) # self._object_pose_t = self._get_current_object_pose() # if self.enable_cameras: # self._camera_observation_t = ( # self._get_current_camera_observation() # ) has_camera_update = True self._object_pose_t = self._get_current_object_pose() if self.enable_cameras: self._camera_observation_t = self._get_current_camera_observation() t = self.simfinger.append_desired_action(action) # The correct timestamp can only be acquired now that t is given. # Update it accordingly in the object and camera observations if has_camera_update: camera_timestamp_s = self.get_timestamp_ms(t) / 1000 self._object_pose_t.timestamp = camera_timestamp_s if self.enable_cameras: for i in range(len(self._camera_observation_t.cameras)): self._camera_observation_t.cameras[ i].timestamp = camera_timestamp_s # write the desired action to the log self._action_log["actions"].append({ "t": t, "torque": action.torque.tolist(), "position": action.position.tolist(), "position_kp": action.position_kp.tolist(), "position_kd": action.position_kd.tolist(), }) return t def _get_current_object_pose(self, t=None): cube_state = self.cube.get_state() pose = ObjectPose() pose.position = np.asarray(cube_state[0]) pose.orientation = np.asarray(cube_state[1]) pose.confidence = 1.0 # NOTE: The timestamp can only be set correctly after time step t is # actually reached. Therefore, this is set to None here and filled # with the proper value later. if t is None: pose.timestamp = None else: pose.timestamp = self.get_timestamp_ms(t) return pose def get_object_pose(self, t): """Get object pose at time step t. Args: t: The time index of the step for which the object pose is requested. Only the value returned by the last call of :meth:`~append_desired_action` is valid. Returns: ObjectPose: Pose of the object. Values come directly from the simulation without adding noise, so the confidence is 1.0. Raises: ValueError: If invalid time index ``t`` is passed. """ current_t = self.simfinger._t if t < 0: raise ValueError("Cannot access time index less than zero.") elif t == current_t: return self._object_pose_t elif t == current_t + 1: return self._get_current_object_pose(t) else: raise ValueError( "Given time index t has to match with index of the current" " step or the next one.") def _get_current_camera_observation(self, t=None): images = self.tricamera.get_images() observation = TriCameraObservation() # NOTE: The timestamp can only be set correctly after time step t # is actually reached. Therefore, this is set to None here and # filled with the proper value later. if t is None: timestamp = None else: timestamp = self.get_timestamp_ms(t) for i, image in enumerate(images): observation.cameras[i].image = image observation.cameras[i].timestamp = timestamp return observation def get_camera_observation(self, t): """Get camera observation at time step t. Args: t: The time index of the step for which the observation is requested. Only the value returned by the last call of :meth:`~append_desired_action` is valid. Returns: TriCameraObservation: Observations of the three cameras. Images are rendered in the simulation. Note that they are not optimized to look realistically. Raises: ValueError: If invalid time index ``t`` is passed. """ if not self.enable_cameras: raise RuntimeError( "Cameras are not enabled. Create `TriFingerPlatform` with" " `enable_cameras=True` if you want to use camera" " observations.") current_t = self.simfinger._t if t < 0: raise ValueError("Cannot access time index less than zero.") elif t == current_t: return self._camera_observation_t elif t == current_t + 1: return self._get_current_camera_observation(t) else: raise ValueError( "Given time index t has to match with index of the current" " step or the next one.") def store_action_log(self, filename): """Store the action log to a JSON file. Args: filename (str): Path to the JSON file to which the log shall be written. If the file exists already, it will be overwritten. """ # TODO should the log also contain intermediate observations (object # and finger) for verification? t = self.simfinger.get_current_timeindex() object_pose = self.get_object_pose(t) self._action_log["final_object_pose"] = { "position": object_pose.position.tolist(), "orientation": object_pose.orientation.tolist(), } with open(filename, "w") as fh: json.dump(self._action_log, fh)
def test_validate_goal(self): on_ground_height = move_cube._CUBOID_HALF_SIZE[2] yaw_rotation = Rotation.from_euler("z", 0.42).as_quat() full_rotation = Rotation.from_euler("zxz", [0.42, 0.1, -2.3]).as_quat() # test some valid goals try: move_cube.validate_goal( move_cube.Pose([0, 0, on_ground_height], [0, 0, 0, 1]) ) except Exception as e: self.fail("Valid goal was considered invalid because %s" % e) try: move_cube.validate_goal( move_cube.Pose([0.05, -0.1, on_ground_height], yaw_rotation) ) except Exception as e: self.fail("Valid goal was considered invalid because %s" % e) try: move_cube.validate_goal( move_cube.Pose([-0.12, 0.0, 0.06], full_rotation) ) except Exception as e: self.fail("Valid goal was considered invalid because %s" % e) # test some invalid goals # invalid values with self.assertRaises(ValueError): move_cube.validate_goal(move_cube.Pose([0, 0], [0, 0, 0, 1])) with self.assertRaises(ValueError): move_cube.validate_goal(move_cube.Pose([0, 0, 0], [0, 0, 1])) # invalid positions with self.assertRaises(move_cube.InvalidGoalError): move_cube.validate_goal( move_cube.Pose([0.3, 0, on_ground_height], [0, 0, 0, 1]) ) with self.assertRaises(move_cube.InvalidGoalError): move_cube.validate_goal( move_cube.Pose([0, -0.3, on_ground_height], [0, 0, 0, 1]) ) with self.assertRaises(move_cube.InvalidGoalError): move_cube.validate_goal(move_cube.Pose([0, 0, 0.3], [0, 0, 0, 1])) with self.assertRaises(move_cube.InvalidGoalError): move_cube.validate_goal(move_cube.Pose([0, 0, 0], [0, 0, 0, 1])) with self.assertRaises(move_cube.InvalidGoalError): move_cube.validate_goal( move_cube.Pose([0, 0, -0.01], [0, 0, 0, 1]) ) # valid CoM position but rotation makes it reach out of valid range with self.assertRaises(move_cube.InvalidGoalError): move_cube.validate_goal( move_cube.Pose([0, 0, on_ground_height], full_rotation) )
def main(logdir, video_path): custom_drawer = CustomDrawer(os.path.join(logdir, 'user/custom_data')) 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=False, 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.CuboidMarker( size=move_cube._CUBOID_SIZE, position=goal['position'], orientation=goal['orientation'], pybullet_client_id=platform.simfinger._pybullet_client_id, ) # if 'grasp_target_cube_pose' in custom_log: # markers.append( # visual_objects.CubeMarker( # width=0.065, # position=custom_log['grasp_target_cube_pose']['position'], # orientation=custom_log['grasp_target_cube_pose']['orientation'], # color=(0, 0, 1, 0.5), # pybullet_client_id=platform.simfinger._pybullet_client_id, # ) # ) # if 'pregrasp_tip_positions' in custom_log: # for tip_pos in custom_log['pregrasp_tip_positions']: # print(tip_pos) # markers.append( # SphereMarker( # radius=0.015, # position=tip_pos, # color=(0, 1, 1, 0.5) # ) # ) # if 'failure_target_tip_positions' in custom_log: # for tip_pos in custom_log['failure_target_tip_positions']: # print(tip_pos) # markers.append( # SphereMarker( # radius=0.015, # position=tip_pos, # color=(1, 0, 0, 0.5) # ) # ) # if 'pitch_grasp_positions' in custom_log: # for tip_pos in custom_log['pitch_grasp_positions']: # print(tip_pos) # markers.append( # SphereMarker( # radius=0.015, # position=tip_pos, # color=(1, 1, 1, 0.5) # ) # ) # if 'yaw_grasp_positions' in custom_log: # for tip_pos in custom_log['yaw_grasp_positions']: # print(tip_pos) # markers.append( # SphereMarker( # radius=0.015, # position=tip_pos, # color=(0, 0, 0, 0.5) # ) # ) 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) custom_drawer.draw(t) 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 test_get_cube_corner_positions(): # cube half width chw = move_cube._CUBE_WIDTH / 2 # no transformation expected_origin_corners = np.array([ [-chw, -chw, -chw], [-chw, -chw, +chw], [-chw, +chw, -chw], [-chw, +chw, +chw], [+chw, -chw, -chw], [+chw, -chw, +chw], [+chw, +chw, -chw], [+chw, +chw, +chw], ]) origin = move_cube.Pose() origin_corners = move_cube.get_cube_corner_positions(origin) np.testing.assert_array_almost_equal(expected_origin_corners, origin_corners) # only translation expected_translated_corners = np.array([ [-chw + 1, -chw + 2, -chw + 3], [-chw + 1, -chw + 2, +chw + 3], [-chw + 1, +chw + 2, -chw + 3], [-chw + 1, +chw + 2, +chw + 3], [+chw + 1, -chw + 2, -chw + 3], [+chw + 1, -chw + 2, +chw + 3], [+chw + 1, +chw + 2, -chw + 3], [+chw + 1, +chw + 2, +chw + 3], ]) translated = move_cube.get_cube_corner_positions( move_cube.Pose([1, 2, 3], [0, 0, 0, 1])) np.testing.assert_array_almost_equal(expected_translated_corners, translated) # only rotation rot_z90 = Rotation.from_euler("z", 90, degrees=True).as_quat() expected_rotated_corners = np.array([ [+chw, -chw, -chw], [+chw, -chw, +chw], [-chw, -chw, -chw], [-chw, -chw, +chw], [+chw, +chw, -chw], [+chw, +chw, +chw], [-chw, +chw, -chw], [-chw, +chw, +chw], ]) rotated = move_cube.get_cube_corner_positions( move_cube.Pose([0, 0, 0], rot_z90)) np.testing.assert_array_almost_equal(expected_rotated_corners, rotated) # both rotation and translation expected_both_corners = np.array([ [+chw + 1, -chw + 2, -chw + 3], [+chw + 1, -chw + 2, +chw + 3], [-chw + 1, -chw + 2, -chw + 3], [-chw + 1, -chw + 2, +chw + 3], [+chw + 1, +chw + 2, -chw + 3], [+chw + 1, +chw + 2, +chw + 3], [-chw + 1, +chw + 2, -chw + 3], [-chw + 1, +chw + 2, +chw + 3], ]) both = move_cube.get_cube_corner_positions( move_cube.Pose([1, 2, 3], rot_z90)) np.testing.assert_array_almost_equal(expected_both_corners, both)
def __init__( self, visualization=False, initial_robot_position=None, initial_object_pose=None, enable_cameras=False, ): """Initialize. Args: visualization (bool): Set to true to run visualization. initial_robot_position: Initial robot joint angles initial_object_pose: Initial pose for the manipulation object. Can be any object with attributes ``position`` (x, y, z) and ``orientation`` (x, y, z, w). This is optional, if not set, a random pose will be sampled. enable_cameras (bool): Set to true to enable camera observations. By default this is disabled as rendering of images takes a lot of computational power. Therefore the cameras should only be enabled if the images are actually used. """ #: Camera rate in frames per second. Observations of camera and #: object pose will only be updated with this rate. #: NOTE: This is currently not used! self._camera_rate_fps = 30 #: Set to true to render camera observations self.enable_cameras = enable_cameras #: Simulation time step self._time_step = 0.004 # first camera update in the first step self._next_camera_update_step = 0 # Initialize robot, object and cameras # ==================================== self.simfinger = SimFinger( finger_type="trifingerpro", time_step=self._time_step, enable_visualization=visualization, ) if initial_robot_position is None: initial_robot_position = self.spaces.robot_position.default self.simfinger.reset_finger_positions_and_velocities( initial_robot_position) if initial_object_pose is None: initial_object_pose = move_cube.Pose( position=self.spaces.object_position.default, orientation=self.spaces.object_orientation.default, ) self.cube = collision_objects.Block( initial_object_pose.position, initial_object_pose.orientation, mass=0.020, ) self.tricamera = camera.TriFingerCameras() # Forward some methods for convenience # ==================================== # forward "RobotFrontend" methods directly to simfinger self.Action = self.simfinger.Action self.get_desired_action = self.simfinger.get_desired_action self.get_applied_action = self.simfinger.get_applied_action self.get_timestamp_ms = self.simfinger.get_timestamp_ms self.get_current_timeindex = self.simfinger.get_current_timeindex self.get_robot_observation = self.simfinger.get_observation # forward kinematics directly to simfinger self.forward_kinematics = ( self.simfinger.pinocchio_utils.forward_kinematics) # Initialize log # ============== self._action_log = { "initial_robot_position": initial_robot_position.tolist(), "initial_object_pose": { "position": initial_object_pose.position.tolist(), "orientation": initial_object_pose.orientation.tolist(), }, "actions": [], }