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