def main():
    argparser = argparse.ArgumentParser(description=__doc__)
    argparser.add_argument(
        "--control-mode",
        default="position",
        choices=["position", "torque"],
        help="Specify position or torque as the control mode.",
    )
    argparser.add_argument(
        "--finger-type",
        default="trifingerone",
        choices=finger_types_data.get_valid_finger_types(),
        help="Specify a valid finger type",
    )
    args = argparser.parse_args()
    time_step = 0.004

    finger = sim_finger.SimFinger(
        finger_type=args.finger_type,
        time_step=time_step,
        enable_visualization=True,
    )
    num_fingers = finger.number_of_fingers

    if args.control_mode == "position":
        position_goals = visual_objects.Marker(number_of_goals=num_fingers)

    while True:

        if args.control_mode == "position":
            desired_joint_positions = np.array(
                sample.random_joint_positions(number_of_fingers=num_fingers))
            finger_action = finger.Action(position=desired_joint_positions)
            # visualize the goal position of the finger tip
            position_goals.set_state(
                finger.pinocchio_utils.forward_kinematics(
                    desired_joint_positions))
        if args.control_mode == "torque":
            desired_joint_torques = [random.random()] * 3 * num_fingers
            finger_action = finger.Action(torque=desired_joint_torques)

        # pursue this goal for one second
        for _ in range(int(1 / time_step)):
            t = finger.append_desired_action(finger_action)
            finger.get_observation(t)
            time.sleep(time_step)
def main_finger(finger_type, data_file):
    finger = SimFinger(enable_visualization=True, finger_type=finger_type)
    goal_marker = visual_objects.Marker(number_of_goals=1)

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

    with open(data_file, "rb") as file_handle:
        episodes = pickle.load(file_handle)

    input("Press enter to start playback")

    start_time_data = None
    start_time_playback = datetime.datetime.now()

    for episode_num, data in enumerate(episodes[1:]):
        print("episode {}".format(episode_num))

        tip_goal = data["tip_goal"][0]
        trajectory = np.vstack(data["joint_positions"])
        timestamps = data["timestamps"]

        if not start_time_data:
            start_time_data = timestamps[0]

        timediff_data = timestamps[0] - start_time_data - 1
        utils.sleep_until(start_time_playback +
                          datetime.timedelta(seconds=timediff_data))
        goal_marker.set_state(tip_goal)
        reset_finger(trajectory[0])

        for position, timestamp in zip(trajectory, timestamps):
            timediff_data = timestamp - start_time_data
            utils.sleep_until(start_time_playback +
                              datetime.timedelta(seconds=timediff_data))

            reset_finger(position)
def main_trifinger(finger_type, data_file_0, data_file_120, data_file_240):

    finger = SimFinger(enable_visualization=True, finger_type=finger_type)
    goal_marker = visual_objects.Marker(number_of_goals=3)

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

    data = []
    for filename in (data_file_0, data_file_120, data_file_240):
        with open(filename, "rb") as file_handle:
            data.append(pickle.load(file_handle))

    assert len(data[0]) == len(data[1]) == len(data[2])

    episodes = []
    for i in range(1, len(data[0])):
        goals = trifinger_goal_space_transforms(finger_type, data)

        initial_position = np.concatenate([
            data[0][i]["joint_positions"][0],
            data[1][i]["joint_positions"][0],
            data[2][i]["joint_positions"][0],
        ])

        joint_positions = []
        for f in range(3):
            for stamp, pos in zip(data[f][i]["timestamps"],
                                  data[f][i]["joint_positions"]):
                joint_positions.append((f, stamp, pos))
        joint_positions = sorted(joint_positions, key=lambda x: x[1])

        episodes.append({
            "tip_goals": goals,
            "joint_positions": joint_positions,
            "initial_position": initial_position,
        })

    input("Press enter to start playback")

    start_time_data = None
    start_time_playback = datetime.datetime.now()

    for episode_num, data in enumerate(episodes):
        print("episode {}".format(episode_num))
        tip_goals = data["tip_goals"]
        trajectory = data["joint_positions"]

        episode_start_time = trajectory[0][1]

        if not start_time_data:
            start_time_data = episode_start_time

        timediff_data = episode_start_time - start_time_data - 1
        utils.sleep_until(start_time_playback +
                          datetime.timedelta(seconds=timediff_data))
        goal_marker.set_state(tip_goals)
        reset_finger(data["initial_position"])

        current_pos = copy.copy(data["initial_position"])

        for finger_idx, timestamp, position in trajectory:
            timediff_data = timestamp - start_time_data
            utils.sleep_until(start_time_playback +
                              datetime.timedelta(seconds=timediff_data))

            slice_start = finger_idx * 3
            current_pos[slice_start:slice_start + 3] = position

            reset_finger(current_pos)
Beispiel #4
0
    def __init__(
        self,
        control_rate_s,
        finger_type,
        enable_visualization,
    ):
        """Intializes the constituents of the pushing environment.

        Constructor sets up the finger robot depending on the finger type,
        sets up the observation and action spaces, smoothing for
        reducing jitter on the robot, and provides for a way to synchronize
        robots being trained independently.


        Args:
            control_rate_s (float): the rate (in seconds) at which step method of the env
                will run. The actual robot controller may run at a higher rate,
                so this is used to compute the number of robot control updates
                per environment step.
            finger_type (string): Name of the finger type.  In order to get
                a list of the valid finger types, call
                :meth:`.finger_types_data.get_valid_finger_types`
            enable_visualization (bool): if the simulation env is to be
                visualized
        """

        #: an instance of the simulated robot depending on the desired
        #: robot type
        self.finger = SimFinger(
            finger_type=finger_type,
            enable_visualization=enable_visualization,
        )

        self.num_fingers = finger_types_data.get_number_of_fingers(finger_type)

        #: the number of times the same action is to be applied to
        #: the robot in one step.
        self.steps_per_control = int(
            round(control_rate_s / self.finger.time_step_s))
        assert (abs(control_rate_s -
                    self.steps_per_control * self.finger.time_step_s) <=
                0.000001)

        #: the types of observations that should be a part of the environment's
        #: observed state
        self.observations_keys = [
            "joint_positions",
            "joint_velocities",
            "action_joint_positions",
            "goal_position",
            "object_position",
        ]
        #: the size of each of the observation type that is part of the
        #: observation keys (in the same order)
        self.observations_sizes = [
            3 * self.num_fingers,
            3 * self.num_fingers,
            3 * self.num_fingers,
            3,
            3,
        ]

        # sets up the observation and action spaces for the environment,
        # unscaled spaces have the custom bounds set up for each observation
        # or action type, whereas all the values in the observation and action
        # spaces lie between 1 and -1
        self.spaces = FingerSpaces(
            num_fingers=self.num_fingers,
            observations_keys=self.observations_keys,
            observations_sizes=self.observations_sizes,
            separate_goals=False,
        )

        self.unscaled_observation_space = (
            self.spaces.get_unscaled_observation_space())
        self.unscaled_action_space = self.spaces.get_unscaled_action_space()

        self.observation_space = self.spaces.get_scaled_observation_space()
        self.action_space = self.spaces.get_scaled_action_space()

        #: a logger to enable logging of observations if desired
        self.logger = DataLogger()

        #: the object that has to be pushed
        self.block = collision_objects.Block()

        #: a marker to visualize where the target goal position for the episode
        #: is
        self.goal_marker = visual_objects.Marker(
            number_of_goals=1,
            goal_size=0.0325,
            initial_position=[0.19, 0.08, 0.0425],
        )

        self.reset()
Beispiel #5
0
    def __init__(
        self,
        control_rate_s,
        finger_type,
        enable_visualization,
        smoothing_params,
        use_real_robot=False,
        finger_config_suffix="0",
        synchronize=False,
    ):
        """Intializes the constituents of the reaching environment.

        Constructor sets up the finger robot depending on the finger type, and
        also whether an instance of the simulated or the real robot is to be
        created. Also sets up the observation and action spaces, smoothing for
        reducing jitter on the robot, and provides for a way to synchronize
        robots being trained independently.

        Args:
            control_rate_s (float): the rate (in seconds) at which step method of the env
                will run. The actual robot controller may run at a higher rate,
                so this is used to compute the number of robot control updates
                per environment step.
            finger_type (string): Name of the finger type.  In order to get
                a dictionary of the valid finger types, call
                :meth:`.finger_types_data.get_valid_finger_types`
            enable_visualization (bool): if the simulation env is to be
                visualized
            smoothing_params (dict):
                num_episodes (int): the total number of episodes for which the
                    training is performed
                start_after (float): the fraction of episodes after which the
                    smoothing of applied actions to the motors should start
                final_alpha (float): smoothing coeff that will be reached at
                    the end of the smoothing
                stop_after (float): the fraction of total episodes by which
                    final alpha is to be reached, after which the same final
                    alpha will be used for smoothing in the remainder of
                    the episodes
                is_test (bool, optional): Include this for testing
            use_real_robot (bool): if training is to be performed on
                the real robot ([default] False)
            finger_config_suffix: pass this if only one of
                the three fingers is to be trained. Valid choices include
                [0, 120, 240] ([default] 0)
            synchronize (bool): Set this to True if you want to train
                independently on three fingers in separate processes, but
                have them synchronized. ([default] False)
        """
        #: an instance of a simulated, or a real robot depending on
        #: what is desired.
        if use_real_robot:
            from pybullet_fingers.real_finger import RealFinger

            self.finger = RealFinger(
                finger_type=finger_type,
                finger_config_suffix=finger_config_suffix,
                enable_visualization=enable_visualization,
            )

        else:
            self.finger = SimFinger(
                finger_type=finger_type,
                enable_visualization=enable_visualization,
            )

        self.num_fingers = finger_types_data.get_number_of_fingers(finger_type)

        #: the number of times the same action is to be applied to
        #: the robot.
        self.steps_per_control = int(
            round(control_rate_s / self.finger.time_step_s))
        assert (abs(control_rate_s -
                    self.steps_per_control * self.finger.time_step_s) <=
                0.000001)

        #: the types of observations that should be a part of the environment's
        #: observed state
        self.observations_keys = [
            "joint_positions",
            "joint_velocities",
            "goal_position",
            "action_joint_positions",
        ]

        self.observations_sizes = [
            3 * self.num_fingers,
            3 * self.num_fingers,
            3 * self.num_fingers,
            3 * self.num_fingers,
        ]

        # sets up the observation and action spaces for the environment,
        # unscaled spaces have the custom bounds set up for each observation
        # or action type, whereas all the values in the observation and action
        # spaces lie between 1 and -1
        self.spaces = FingerSpaces(
            num_fingers=self.num_fingers,
            observations_keys=self.observations_keys,
            observations_sizes=self.observations_sizes,
            separate_goals=True,
        )

        self.unscaled_observation_space = (
            self.spaces.get_unscaled_observation_space())
        self.unscaled_action_space = self.spaces.get_unscaled_action_space()

        self.observation_space = self.spaces.get_scaled_observation_space()
        self.action_space = self.spaces.get_scaled_action_space()

        #: a logger to enable logging of observations if desired
        self.logger = DataLogger()

        # sets up smooothing
        if "is_test" in smoothing_params:
            self.smoothing_start_episode = 0
            self.smoothing_alpha = smoothing_params["final_alpha"]
            self.smoothing_increase_step = 0
            self.smoothing_stop_episode = math.inf
        else:
            self.smoothing_stop_episode = int(
                smoothing_params["num_episodes"] *
                smoothing_params["stop_after"])

            self.smoothing_start_episode = int(
                smoothing_params["num_episodes"] *
                smoothing_params["start_after"])
            num_smoothing_increase_steps = (self.smoothing_stop_episode -
                                            self.smoothing_start_episode)
            self.smoothing_alpha = 0
            self.smoothing_increase_step = (smoothing_params["final_alpha"] /
                                            num_smoothing_increase_steps)

        self.smoothed_action = None
        self.episode_count = 0

        #: a marker to visualize where the target goal position for the episode
        #: is to which the tip link(s) of the robot should reach
        self.enable_visualization = enable_visualization
        if self.enable_visualization:
            self.goal_marker = visual_objects.Marker(
                number_of_goals=self.num_fingers)

        # set up synchronization if it's set to true
        self.synchronize = synchronize
        if synchronize:
            now = datetime.datetime.now()
            self.next_start_time = datetime.datetime(now.year, now.month,
                                                     now.day, now.hour,
                                                     now.minute + 1)
        else:
            self.next_start_time = None

        self.seed()
        self.reset()