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__(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 __init__(self, simulator, states, world=None, terminal_condition='default'): r"""Initialize the locomotion environment Args: simulator (Simulator): simulator states (State, Policy): states that environment must return. If the policy is given, it takes the states that are given as input to the policy. world (World, None): the world for the locomotion task. If None, it creates a basic world. terminal_condition (str, default): the terminating condition criterion to stop if the policy failed the task. """ # check parameters if not isinstance(simulator, Simulator): raise TypeError( "Expecting the `simulator` parameter to be an instance of Simulator." ) if not isinstance(states, (State, Policy)): raise TypeError( "Expecting the `states` parameter to be an instance of State or Policy." ) # define world if world is None: world = BasicWorld(simulator) # get states/actions if policy actions = None if isinstance(states, Policy): actions = states.actions states = states.states # define reward based on states/actions rewards = None # define terminating condition criterion if terminal_condition == 'default': terminal_condition = None terminal_condition = None super(LocomotionEnv, self).__init__(world, states, rewards=rewards, terminal_conditions=terminal_condition, extra_info=None)
def __init__(self, sim, states=None, rewards=None, terminal_conditions=None, initial_state_generators=None, physics_randomizers=None, extra_info=None, actions=None): world = BasicWorld(sim) super(BasicEnv, self).__init__(world, states, rewards, terminal_conditions, initial_state_generators, physics_randomizers, extra_info, actions)
#!/usr/bin/env python """Inverse kinematics with the Kuka robot where the goal is to follow a moving sphere. """ import numpy as np from itertools import count from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import KukaIIWA, Body # Create simulator sim = Bullet() # create world world = BasicWorld(sim) # create robot robot = KukaIIWA(sim) robot.print_info() world.load_robot(robot) # define useful variables for IK dt = 1. / 240 link_id = robot.get_end_effector_ids(end_effector=0) joint_ids = robot.joints # actuated joint damping = 0.01 # for damped-least-squares IK wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1') qIdx = robot.get_q_indices(joint_ids) # define gains
if isinstance(values, (int, float)): values = np.ones(len(self.motors)) * values pass # Test if __name__ == "__main__": from itertools import count from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld # Create simulator sim = Bullet() # create world world = BasicWorld(sim) # create robot robots = [] for _ in range(30): x, y = np.random.uniform(low=-1, high=1, size=2) robot = world.load_robot(Kilobot, position=(x, y, 0)) robots.append(robot) # print information about the robot robots[0].print_info() # Position control using sliders # robots[0].add_joint_slider() # run simulator
from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import KukaIIWA X_desired = np.array([0., 0., 1.3]) speed_scale_factor = 0.05 if __name__ == "__main__": # Initialize XBox controller controller = XboxControllerInterface(use_thread=True, verbose=False) # Create simulator sim = Bullet() # create world world = BasicWorld(sim) # create robot robot = KukaIIWA(sim) robot.print_info() # define useful variables for IK dt = 1. / 240 link_id = robot.get_end_effector_ids(end_effector=0) joint_ids = robot.joints # actuated joint # joint_ids = joint_ids[2:] damping = 0.01 # for damped-least-squares IK wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1') # change the robot visual # robot.change_transparency()
'1: use damped-least-squares IK using Jacobian)', type=int, choices=[0, 1], default=1) args = parser.parse_args() # select IK solver, by setting the flag: # 0 = pybullet + calculate_inverse_kinematics() # 1 = pybullet + damped-least-squares IK using Jacobian (provided by pybullet) solver_flag = args.solver # 1 gives a pretty good result # Create simulator sim = Bullet() # create world world = BasicWorld(sim) # create robot robot = KukaIIWA(sim) robot.print_info() # define useful variables for IK dt = 1. / 240 link_id = robot.get_end_effector_ids(end_effector=0) joint_ids = robot.joints # actuated joint # joint_ids = joint_ids[2:] damping = 0.01 # for damped-least-squares IK wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1') # desired position xd = np.array([0.5, 0., 0.5])
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
# taken from "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019 self.kp = 50. * np.ones(12) self.kd = 0.1 * np.ones(12) # Test if __name__ == "__main__": from itertools import count from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld # Create simulator sim = Bullet() # create world world = BasicWorld(sim) # world.load_japanese_monastery() # create robot robot = HyQ2Max(sim) # print information about the robot robot.print_info() # Position control using sliders # robot.add_joint_slider(robot.getLeftFrontLegIds()) # run simulator for i in count(): # robot.update_joint_slider() robot.compute_and_draw_com_position()
def __getattr__(self, item): """The Gym Env have the same methods and attributes as the PRL Env.""" return getattr(self.env, item) # Tests if __name__ == '__main__': from pyrobolearn.simulators import Bullet import time # create simulator sim = Bullet() # create world world = BasicWorld(sim) robot = world.load_robot('coman', fixed_base=True) # create states states = State() # create rewards reward = Reward() # create Env env = Env(world, states, reward) # dummy action action = Action() # run the environment for n steps
The Kuka robot just draw a circle in the air. The joint positions are in the `data.txt` file. """ import os import pickle from itertools import count from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import KukaIIWA, Body # Create simulator sim = Bullet() # create world world = BasicWorld(sim) # create robot robot = KukaIIWA(sim) robot.print_info() # define useful variables for FK link_id = robot.get_end_effector_ids(end_effector=0) joint_ids = robot.joints # actuated joint # load data with open(os.path.dirname(os.path.abspath(__file__)) + '/data.txt', 'rb') as f: positions = pickle.load(f) # set initial joint position robot.reset_joint_states(q=positions[0])
default=False, action='store_true', help="use stadium for world") args = parser.parse_args() RATE_HZ = args.fps TIME_STEP = 1.0 / RATE_HZ GRAVITY_MSS = 9.80665 # Create simulator sim = Bullet() # create world from pyrobolearn.worlds import BasicWorld world = BasicWorld(sim) if args.stadium: world.sim.remove_body(world.floor_id) world.floor_id = world.sim.load_sdf(os.path.join( pybullet_data.getDataPath(), "stadium.sdf"), position=[0, 0, 0]) # setup keyboard interface interface = prl.tools.interfaces.MouseKeyboardInterface() def control_quad(pwm): '''control quadcopter''' motor_dir = [1, 1, -1, -1] motor_order = [0, 1, 2, 3]
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)
# -*- coding: utf-8 -*- #!/usr/bin/env python """Provide the HyQ2Max robot. """ from itertools import count from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import HyQ2Max # Create simulator sim = Bullet() # create world world = BasicWorld(sim) world.load_japanese_monastery() # create robot robot = HyQ2Max(sim) # print information about the robot robot.print_info() # Position control using sliders # robot.add_joint_slider(robot.getLeftFrontLegIds()) # run simulator for _ in count(): # robot.update_joint_slider() robot.compute_and_draw_com_position() robot.compute_and_draw_projected_com_position()
from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld import time from itertools import count # create simulator sim = Bullet() # sim.configure_debug_visualizer(p.COV_ENABLE_GUI, 0) # sim.configure_debug_visualizer(p.COV_ENABLE_RENDERING, 0) # sim.configure_debug_visualizer(p.COV_ENABLE_TINY_RENDERER, 1) # sim.configure_debug_visualizer(p.COV_ENABLE_WIREFRAME, 1) # sim.configure_debug_visualizer(p.COV_ENABLE_Y_AXIS_UP, 0) # sim.configure_debug_visualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) # sim.configure_debug_visualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) # sim.configure_debug_visualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) # create World world = BasicWorld(sim) # load robot robot = world.load_robot('baxter', fixed_base=True) # create bridge/interface bridge = BridgeMouseKeyboardWorld(world, verbose=True) for _ in count(): bridge.step(update_interface=True) # world.step() time.sleep(1. / 100)
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)
'--init_config', help='the initial configuration for the robot', type=int, choices=range(1, 5), default=1) args = parser.parse_args() # program variable robot_name = args.robot dt = 0.01 # Sampling time num_des_manip = args.init_manipulability # Number of desired manipulability (Useful for tests) init_qs = args.init_config # Number of initial configuration for the robots (Useful for tests) # 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':
parser.add_argument('-n', '--init_manipulability', help='which desired velocity manipulability we want to track', type=int, default=4) parser.add_argument('-i', '--init_config', help='the initial configuration for the robot', type=int, choices=range(1, 5), default=1) args = parser.parse_args() # program variable robot_name = args.robot dt = 0.01 # Sampling time num_des_manip = args.init_manipulability # Number of desired manipulability (Useful for tests) init_qs = args.init_config # Number of initial configuration for the robots (Useful for tests) # 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':
#!/usr/bin/env python # -*- coding: utf-8 -*- """Load the WALK-MAN robot. """ from itertools import count from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Walkman # Create simulator sim = Bullet() # Create world world = BasicWorld(sim) world.load_sphere([2., 0, 2.], mass=0., color=(1, 0, 0, 1)) world.load_sphere([2., 1., 2.], mass=0., color=(0, 0, 1, 1)) # load robot robot = Walkman(sim, fixed_base=False, lower_body=False) # print information about the robot robot.print_info() # # Position control using sliders robot.add_joint_slider(robot.left_leg) # run simulator for i in count(): robot.update_joint_slider() if i % 60 == 0:
from pyrobolearn.tools.interfaces import MouseKeyboardInterface from pyrobolearn.tools.bridges import BridgeMouseKeyboardImitationTask from pyrobolearn.recorders import StateRecorder, ActionRecorder from pyrobolearn.tasks import ILTask # variables joint_ids = None # None for all the actuated joints, or you can select which joint you want to move; e.g. [0, 1, 2] num_basis = 20 rate = 30 # Create simulator sim = Bullet() # create world world = BasicWorld(sim) # load robot in the world robot = world.load_robot(KukaIIWA) print("Robot's actuated joint ids: {}".format(robot.joints)) # create state/action state = ExponentialPhaseState(ticks=rate) action = JointPositionAction(robot, joint_ids=joint_ids) print("State: {}".format(state)) print("Action: {}".format(action)) # create environment env = Env(world, state) # create DMP policy
# -*- coding: utf-8 -*- #!/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)
break # step in simulation world.step(sleep_dt=dt) plt.plot(num, sp_z) # plot the position on the z axis plt.xlabel("timesteps") plt.ylabel("vertical position") plt.title("The z axis position during the task") plt.show() if __name__ == '__main__': # Create simulator sim = Bullet() # create world world = BasicWorld(sim) # flag : 0 # PI control flag = 0 # create robot robot = KukaIIWA(sim) robot.print_info() world.load_robot(robot) world.load_table(position=np.array([1, 0., 0.]), orientation=np.array([0.0, 0.0, 0.0, 1.0])) # define useful variables for IK dt = 1. / 240 link_id = robot.get_end_effector_ids(end_effector=0) joint_ids = robot.joints # actuated joint damping = 0.01 # for damped-least-squares IK wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1')