#!/usr/bin/env python
"""Load the Minitaur robot.
"""

from itertools import count
from pyrobolearn.simulators import Bullet
from pyrobolearn.worlds import BasicWorld
from pyrobolearn.robots import Minitaur

# Create simulator
sim = Bullet()

# create world
world = BasicWorld(sim)

# create robot
robot = Minitaur(sim)

# print information about the robot
robot.print_info()

# Position control using sliders
# robot.add_joint_slider(robot.left_front_leg)

# run simulator
for _ in count():
    # robot.update_joint_slider()
    # robot.compute_and_draw_com_position()
    # robot.compute_and_draw_projected_com_position()
    world.step(sleep_dt=1./240)
Beispiel #2
0
# Create simulator and world
sim = Bullet()
world = BasicWorld(sim)

# load robot
if robot_name == 'nao':
    robot = Nao(sim, fixed_base=False)
elif robot_name == 'centauro':
    robot = Centauro(sim, fixed_base=False)
else:
    raise NotImplementedError("The given robot has not been implemented")

# Loop for setting stable initial conditions
for i in range(50):
    world.step(sleep_dt=0.1)

# Define velocity manipulability for CoM, proportional gain, and initial configurations
if robot.name == 'nao':
    Km = 100 * np.eye(6)  # Proportional gain for Nao for manip. tracking

    # desired Velocity Manipulability for CoM (Nao)
    des_manips = np.array([[[1.539e-03, 6.653e-04, 0.833e-04],
                            [6.653e-04, 2.080e-03, -5.843e-05],
                            [0.833e-04, -5.843e-05, 8.601e-04]],
                           [[2.580e-03, 1.653e-03, 4.833e-04],
                            [1.653e-03, 1.539e-03, -5.843e-05],
                            [4.833e-04, -5.843e-05, 9.601e-04]],
                           [[1.580e-03, .653e-03, 4.833e-04],
                            [.653e-03, 1.539e-03, -5.843e-05],
                            [4.833e-04, -5.843e-05, 9.601e-04]],
    sphere.position = np.array([
        0.5, r * np.cos(w * t + np.pi / 2),
        (1. - r) + r * np.sin(w * t + np.pi / 2)
    ])

    # get current end-effector position and velocity in the task/operational space
    x = robot.get_link_world_positions(link_id)
    dx = robot.get_link_world_linear_velocities(link_id)

    # Get joint positions
    q = robot.get_joint_positions()

    # Get linear jacobian
    if robot.has_floating_base():
        J = robot.get_linear_jacobian(link_id, q=q)[:, qIdx + 6]
    else:
        J = robot.get_linear_jacobian(link_id, q=q)[:, qIdx]

    # Pseudo-inverse: \hat{J} = J^T (JJ^T + k^2 I)^{-1}
    Jp = robot.get_damped_least_squares_inverse(J, damping)

    # evaluate damped-least-squares IK
    dq = Jp.dot(kp * (sphere.position - x) - kd * dx)

    # set joint positions
    q = q[qIdx] + dq * dt
    robot.set_joint_positions(q, joint_ids=joint_ids)

    # step in simulation
    world.step(sleep_dt=dt)
Beispiel #4
0
            t.join()
        # stop connection
        self.connection.close()
        self.sock.close()


# Test
if __name__ == "__main__":
    from pyrobolearn.simulators import Bullet
    from pyrobolearn.worlds import BasicWorld
    import time
    import numpy as np
    from itertools import count

    # create simulator
    sim = Bullet()

    # create world
    world = BasicWorld(sim)

    # create interface
    interface = OculusInterface(world, port=5111)

    # run simulation
    for t in count():
        interface.step()
        # interface.print_state()
        # step in the simulation
        world.step()
        time.sleep(1. / 60)
Beispiel #5
0
class DemoRecEnv:
    def __init__(self, demo_dt, demo_dir):

        # Initialize XBox controller
        self.controller = XboxControllerInterface(use_thread=True,
                                                  verbose=False)

        # Create simulator
        self.sim = Bullet()

        # create world
        self.world = BasicWorld(self.sim)

        # Create Robot
        self.robot = Robot(self.sim, self.world)
        self.robot.go_home()

        self.workspace_outer_bound = {'rad': 1.0}
        self.workspace_inner_bound = {'rad': 0.25, 'height': 0.7}

        self.env_objects = []
        self.demo_dir = demo_dir
        self.demo_dt = demo_dt

    def init_task_recorder(self):
        num_viapts = 2  # int(input("How many via points?"))
        via_pts = []

        for i in range(num_viapts):
            pt = generate_pt(via_pts, self.workspace_outer_bound,
                             self.workspace_inner_bound)
            via_pts.append(pt)
            self.env_objects.append(
                self.world.load_visual_sphere(pt,
                                              radius=0.05,
                                              color=(1, 0, 0, 1)))

        print("Via-pts generated.")
        print(
            "Press 'start' to start/stop recording demonstrations,select to end training and Y to reset via pts"
        )
        while True:
            last_trigger = self.controller.last_updated_button
            trigger_value = self.controller[last_trigger]

            if last_trigger == 'menu' and trigger_value == 1:
                self.start_recording()
            elif last_trigger == 'view' and trigger_value == 1:
                print("Ending Training")
                self.reset_env()
                break
            elif last_trigger == 'Y' and trigger_value == 1:
                for obj in self.env_objects:
                    self.world.sim.remove_body(obj)
                self.init_task_recorder()
        return

    def start_recording(self):
        time.sleep(1)
        rec_traj = []
        print(
            "Started Recording. Press 'start' again to stop recording and 'A' to send robot to end"
        )

        sphere = None
        X_desired = self.robot.robot.sim.get_link_world_positions(
            body_id=self.robot.robot.id, link_ids=self.robot.ee_id)
        speed_scale_factor = 0.01
        time_ind = 0
        save_every = 3

        for t in count():
            last_trigger = self.controller.last_updated_button
            trigger_value = self.controller[last_trigger]
            movement = self.controller[last_trigger]

            if last_trigger == 'menu' and trigger_value == 1:
                print("Stopping recording and resetting environment")
                self.save_traj(rec_traj)
                self.reset_env()
            elif last_trigger == 'A' and trigger_value == 1:
                X_desired = self.robot.end_pos
            # Update target robot location from controller input
            elif last_trigger == 'LJ':
                X_desired[0] = X_desired[0] + round(movement[0],
                                                    1) * speed_scale_factor
                X_desired[1] = X_desired[1] + round(movement[1],
                                                    1) * speed_scale_factor
            elif last_trigger == 'RJ':
                X_desired[2] = X_desired[2] + round(movement[1],
                                                    1) * speed_scale_factor

            # Record points every 10 iterations
            if not t % save_every:
                x = self.robot.robot.sim.get_link_world_positions(
                    body_id=self.robot.robot.id, link_ids=self.robot.ee_id)
                dx = self.robot.robot.get_link_world_linear_velocities(
                    self.robot.ee_id)
                rec_traj.append((time_ind * self.demo_dt, ) +
                                tuple(round(i, 3) for i in x) +
                                tuple(round(i, 3) for i in dx))
                time_ind = time_ind + 1

            # Visualize target EE location
            if sphere:
                self.world.sim.remove_body(sphere)
            sphere = self.world.load_visual_sphere(X_desired,
                                                   radius=0.01,
                                                   color=(0, 1, 0, 1))

            # Send robot to target location
            self.robot.send_robot_to(X_desired)

            self.world.step()

    def reset_env(self):
        self.robot.go_home()
        self.remove_objects()
        self.init_task_recorder()

    def remove_objects(self):
        for obj in self.env_objects:
            self.world.sim.remove_body(obj)

    def step_env(self):
        self.controller.step()
        self.world.step()

    def save_traj(self, rec_traj):
        # Finding the index of last file saved
        print(rec_traj[:10])
        listdir = os.listdir(self.demo_dir)
        listdir = list(map(int, listdir))
        listdir.sort()
        # print(listdir)
        ind_last_file = 0 if len(listdir) == 0 else int(listdir[-1])
        # print(ind_last_file)
        del (rec_traj[0])
        with open(self.demo_dir + str(ind_last_file + 1), 'w') as f:
            write = csv.writer(f)
            write.writerows(rec_traj)
Beispiel #6
0
class Environment:
    def __init__(self, spheres=[[0.09, -0.44, 0.715, 1]], cuboids=None):
        # Define size of the map
        self.res = 1.
        # Initialize XBox controller
        self.controller = XboxControllerInterface(use_thread=True,
                                                  verbose=False)
        # Create simulator
        self.sim = Bullet()
        # create world
        self.world = BasicWorld(self.sim)
        # create robot
        self.robot = KukaIIWA(self.sim)
        # define start/end pt
        self.start_pt = [-0.75, 0., 0.75]
        self.end_pt = [0.75, 0., 0.75]
        # Initialize robot location
        self.send_robot_to(np.array(self.start_pt), dt=2)

        # Get via points from user
        via_pts = self.get_via_pts(
        )  # [[-0.19, -0.65, 0.77], [0.51, -0.44, 0.58]]
        self.motion_targets = [self.start_pt] + via_pts + [self.end_pt]

    def send_robot_to(self, location, dt):
        start = time.perf_counter()
        for _ in count():
            joint_ids = self.robot.joints
            link_id = self.robot.get_end_effector_ids(end_effector=0)
            qIdx = self.robot.get_q_indices(joint_ids)

            x = self.robot.sim.get_link_world_positions(body_id=self.robot.id,
                                                        link_ids=link_id)
            q = self.robot.calculate_inverse_kinematics(link_id,
                                                        position=location)
            self.robot.set_joint_positions(q[qIdx], joint_ids)
            self.world.step()

            if time.perf_counter() - start >= dt:
                break

    def get_via_pts(self):
        print(
            "Move green dot to via point. Press 'A' to save. Press 'X' to finish."
        )

        X_desired = [0., 0., 1.3]
        speed_scale_factor = 0.01
        via_pts = []

        sphere = None
        for _ in count():

            last_trigger = self.controller.last_updated_button
            movement = self.controller[last_trigger]

            if last_trigger == 'LJ':
                X_desired[0] = X_desired[0] + round(movement[0],
                                                    1) * speed_scale_factor
                X_desired[1] = X_desired[1] + round(movement[1],
                                                    1) * speed_scale_factor
            elif last_trigger == 'RJ':
                X_desired[2] = X_desired[2] + round(movement[1],
                                                    1) * speed_scale_factor
            elif last_trigger == 'A' and movement == 1:
                print("Via point added")
                via_pts.append(X_desired)
                time.sleep(0.5)
            elif last_trigger == 'X' and movement == 1:
                self.world.sim.remove_body(sphere)
                print("Via point generation complete.")
                break

            if sphere:
                self.world.sim.remove_body(sphere)
            sphere = self.world.load_visual_sphere(X_desired,
                                                   radius=0.01,
                                                   color=(0, 1, 0, 1))

        return via_pts