def __init__(
self,
debug=False,
steps=120,
control_freq=CONTROL_FREQUENCY,
with_arm=False,
human_render_duration=10,
img_res=(84, 84),
):
super().__init__()
self.debug = debug
## observations:
## 0 - timer, [0,1], corresponding to relative no of episode steps
## 1 - sin(timer), [-1,1]
## 2 - cos(timer), [-1,1],
## 3,4,5 - position of base, xyz, absolute
## 6,7,8 - orientation of base, euler angles, [-1,1]
## 9,10,11 - velocity of base, xyz, absolute
## 12..24 - positions of feet FR,FL,BR,BL, in xyz order, [-1,1]
self.observation_space = spaces.Box(low=-1,
high=1,
shape=(24, ),
dtype=np.float32)
# action = 12 foot positions, corresponding to FR,FL,BR,BL and each with xyz, so [FR_x,FR_y,FR_Z,FL_x,FL_y,...
self.action_space = spaces.Box(low=-1,
high=1,
shape=(12, ),
dtype=np.float32)
self.sim = PupperSim2(debug=debug,
start_standing=False,
with_arm=with_arm,
with_feet=True,
img_size=img_res)
self.orn_start_up, self.orn_start_fw = self._get_upfw_orientation()
self.episode_steps = 0
self.episode_steps_max = steps
self.control_freq = control_freq
self.human_render_duration = human_render_duration
self.dt = 1 / self.control_freq
self.sim_steps = int(round(FREQ_SIM / control_freq))
# orientation - this is only relevant for IK and we assume open-loop control for now wrt IMU
self.quat = np.array([1, 0, 0, 0])
import math
import time
import numpy as np
from gym_pupper.sim.simulator2 import PupperSim2
FREQ_SIM = 240 # arbitrary but should be a multiple of the RECORDING_FPS
RESOLUTION = 256
FREQ_CTRL = 60
substeps = int(FREQ_SIM / FREQ_CTRL)
sim = PupperSim2(debug=True,
img_size=(RESOLUTION, RESOLUTION),
frequency=FREQ_SIM,
with_arm=True)
sim.reset(rest=True)
sim.change_color((0, 1, 1, 1))
time.sleep(2)
for _ in range(10):
sim.step()
motors = [24, 26, 28]
# Container for debug inputs
debugParams = []
## safe arm resting pos: [0, 1, -.85] * pi/2
# In the user interface, create a slider for each motor to control them separately.
from gym_pupper.sim.simulator2 import PupperSim2
STEP_WIDTH = 2 # if you're standing in front of a staircase and looking up, what's the left-right distance?
STEP_DEPTH = 0.3 # if you're stepping onto one step, how much of your foot can fit on the step?
STEP_HEIGHT = 0.05 # how far for you have to go up between each step?
OFFSET = (0.3, 0, 0)
sim = PupperSim2(debug=True)
sim.reset()
sim.add_rooms()
for step in range(100000):
# sim.action(motorPo)
sim.step()
def __init__(
self,
debug=False,
steps=120,
control_freq=CONTROL_FREQUENCY,
action_scaling=1.0,
random_rot=(0, 0, 0),
reward_coefficients=(0.1, 1, 1),
stop_on_flip=False,
):
""" Gym-compatible environment to teach the pupper how to walk - with predefined gait as baseline
"""
super().__init__()
# observation space:
# - 12 lef joints in the order
# - front right hip
# - front right upper leg
# - front right lower leg
# - front left hip/upper/lower leg
# - back right hip/upper/lower leg
# - back left hip/upper/lower leg
# - 3 body orientation in euler angles
# - 2 linear velocity (only along the plane, we don't care about z velocity
self.observation_space = spaces.Box(low=-1, high=1, shape=(12 + 3 + 2,), dtype=np.float32)
# action = x/y/z offset for all 4 feet
self.action_space = spaces.Box(low=-1, high=1, shape=(12,), dtype=np.float32)
# turning off start_standing because the that's done in self.reset()
kwargs = {}
# if relative_action: # if this is turned on, the robot is underpowered for the hardcoded gait
# kwargs = {"gain_pos": 1 / 16, "gain_vel": 1 / 8, "max_torque": 1 / 2}
self.sim = PupperSim2(debug=debug, start_standing=False, **kwargs)
self.episode_steps = 0
self.episode_steps_max = steps
self.control_freq = control_freq
self.dt = 1 / self.control_freq
self.incremental_angle = np.deg2rad(MAX_ANGLE_PER_SEC) / self.control_freq
self.sim_steps = int(round(FREQ_SIM / control_freq))
print(f"Running with {self.sim_steps} substeps")
self.action_scaling = action_scaling
self.random_rot = random_rot
self.stop_on_flip = stop_on_flip
self.current_action = np.array([0] * 12)
self.gait = None
ranges = np.load(f"{ASSET_DIR}/joint_foot_ranges.npz")
self.foot_ranges = ranges["foot_ranges"].reshape(12, 2)
self.foot_ranges_diff = self.foot_ranges[:, 1] - self.foot_ranges[:, 0]
# extend the allowed foot ranges by 25% of the range on that axis
print("fr before", self.foot_ranges)
self.foot_ranges = np.array(
[
(fr[0] - 0.25 * fd, fr[1] + 0.25 * fd) if fd > 0 else (fr[0] - 0.1, fr[1] + 0.1)
for fr, fd in zip(self.foot_ranges, self.foot_ranges_diff)
]
)
self.foot_ranges_diff[self.foot_ranges_diff == 0] = 0.2 # corresponding to min dist
print("fr after", self.foot_ranges)
self.joint_ranges = ranges["joint_ranges"]
print("joint ranges", self.joint_ranges)
# new reward coefficients
self.rcoeff_ctrl, self.rcoeff_run, self.rcoeff_stable = reward_coefficients
FPS = 60
history_length = 5
substeps = int(240 / FPS)
steps = 2000
tick = 0
joints = []
feet = []
quat = np.array([1, 0, 0, 0])
config = Configuration(dt=1 / FPS)
controller = Controller(config)
foot_locations = DEFAULT_STANCE + np.array([0, 0, DEFAULT_Z_REF])[:,
np.newaxis]
sim = PupperSim2(with_arm=False,
frequency=240,
debug=True,
img_size=(512, 256))
sim.make_kinematic(
(0.588, 0.419, 1, 1)) # use with sim.set(), otherwise use sim.action()
sim.reset()
# sim.action_arm([0, 0, 0])
# sim.change_color((0.588, 0.419, 1, 1))
foot_history = deque([np.zeros((4, 3))] * history_length)
app = pg.mkQApp("Plotting Example")
win = pg.GraphicsLayoutWidget(show=True, title="Basic plotting examples")
win.resize(1000, 600)
win.setWindowTitle("pyqtgraph example: Plotting")
pg.setConfigOptions(antialias=True)
import os
import time
import numpy as np
from gym_pupper.assets import ASSET_DIR
from gym_pupper.sim.simulator2 import PupperSim2, REST_POS
sim = PupperSim2(debug=True, with_feet=True)
debugParams = []
rest_pos = REST_POS.T.flatten()
for i in range(len(sim.joint_indices)):
if (i + 1) % 3 == 0:
motor = sim.p.addUserDebugParameter("motor{}".format(i + 1),
-np.pi + 0.001, np.pi / 2,
rest_pos[i])
else:
motor = sim.p.addUserDebugParameter("motor{}".format(i + 1),
-np.pi / 2, np.pi / 2, rest_pos[i])
debugParams.append(motor)
finish = sim.p.addUserDebugParameter("finish", 0, 1, 0)
debugParams.append(finish)
print(sim.get_pos_orn_vel())
print(sim.get_joint_states())
sim.make_kinematic()
positions = []
import cv2
import numpy as np
import gym
from tqdm import trange
import gym_pupper
from gym_pupper.common.Utilities import controller_to_sim, ik_to_sim
from gym_pupper.pupper.policy import HardPolicy
from gym_pupper.sim.simulator2 import PupperSim2
FREQ_SIM = 240
FREQ_CTRL = 60
substeps = int(FREQ_SIM / FREQ_CTRL)
sim = PupperSim2(with_arm=True,
frequency=FREQ_SIM,
debug=True,
img_size=(512, 256))
sim.reset()
sim.change_color((0.588, 0.419, 1, 1))
# time.sleep(1)
policy = HardPolicy(fps=FREQ_CTRL, warmup=10)
arm_rest = np.array([0, 1, -0.8]) * np.pi / 2
sim.action_arm(arm_rest)
arm_angles = []
for i in trange(60 * 47):
# action, _ = controller_to_sim(policy.act(velocity_horizontal=(-0.2, 0), normalized=True)) * np.pi
# action, _ = controller_to_sim(policy.act(velocity_horizontal=(0.2, 0), yaw_rate=1.0, normalized=True)) * np.pi
# idle_walk_fw = 0.008
import time
import numpy as np
import matplotlib.pyplot as plt
from gym_pupper.common.Utilities import controller_to_sim
from gym_pupper.pupper.policy import HardPolicy
from gym_pupper.sim.simulator2 import PupperSim2
FPS = 60 # fraction of 240
substeps = int(round(240 / FPS))
sim = PupperSim2(debug=True, frequency=240)
shelf = sim.p.loadURDF(
"shelf.urdf.xml", basePosition=[1, 0, 0], baseOrientation=sim.p.getQuaternionFromEuler([0, 0, np.pi / 2])
)
sim.p.changeDynamics(shelf, -1, linearDamping=0, angularDamping=0)
sim.p.setCollisionFilterGroupMask(shelf, -1, collisionFilterGroup=0, collisionFilterMask=0)
sleepy_state = (
sim.p.ACTIVATION_STATE_SLEEP + sim.p.ACTIVATION_STATE_ENABLE_SLEEPING + sim.p.ACTIVATION_STATE_DISABLE_WAKEUP
)
sim.p.changeDynamics(shelf, -1, activationState=sleepy_state)
def reset_sim():
sim.reset(rest=True)
sim.change_color((0, 1, 1, 1))
for _ in range(10):
sim.step()
substeps = int(round(SIM_FPS / RECORDING_FPS))
f = h5py.File(f"/Users/florian/dev/pyvicon/scripts/pupper-{TRICK}.hdf5", "r")
vicon_positions = f["vicon_positions"]
vicon_rotations = f["vicon_rotations"]
joints = f["joints"]
feet = f["foot_positions"]
########
# KINEMATIC SIM
########
sim_ref = PupperSim2(debug=False, img_size=(RESOLUTION, RESOLUTION), frequency=SIM_FPS)
sim_ref.reset()
# make the robot limp for demonstration
sim_ref.make_kinematic()
sim_learner = PupperSim2(debug=False, img_size=(RESOLUTION, RESOLUTION), frequency=SIM_FPS)
sim_learner.reset(rest=True)
sim_learner.change_color((0, 1, 1, 1))
for _ in range(10):
sim_learner.step()
base_pos = vicon_positions[FRAME_START]
base_rot = vicon_rotations[FRAME_START]
def policy(_):
from gym_pupper.sim.simulator2 import PupperSim2, REST_POS
RECORDING_FPS = 60 # limited by camera
SIM_FPS = 240 # arbitrary but should be a multiple of the RECORDING_FPS
TRICK = "walk-forward"
FRAME_START = 90
FRAME_END = 400
RESOLUTION = 256
substeps = int(round(SIM_FPS / RECORDING_FPS))
sim = PupperSim2(debug=True,
img_size=(RESOLUTION, RESOLUTION),
frequency=SIM_FPS)
sim.reset(rest=True)
sim.make_kinematic()
roll = sim.p.addUserDebugParameter("roll", -np.pi, np.pi, 0)
pitch = sim.p.addUserDebugParameter("pitch", -np.pi, np.pi, 0)
yaw = sim.p.addUserDebugParameter("yaw", -np.pi, np.pi, 0)
txt = None
while True:
if txt is not None:
sim.p.removeUserDebugItem(txt)
substeps = int(round(SIM_FPS / RECORDING_FPS))
f = h5py.File(f"/Users/florian/dev/pyvicon/scripts/pupper-{TRICK}.hdf5", "r")
vicon_positions = f["vicon_positions"]
vicon_rotations = f["vicon_rotations"]
joints = f["joints"]
feet = f["foot_positions"]
########
# KINEMATIC SIM
########
sim = PupperSim2(debug=False, img_size=(256, 256), frequency=SIM_FPS)
# move the robot down to the ground
sim.reset(rest=True)
for _ in range(10):
sim.step()
# make the robot limp for demonstration
sim.make_kinematic()
base_pos = vicon_positions[FRAME_START]
base_rot = vicon_rotations[FRAME_START]
for frame_idx in range(FRAME_START, FRAME_END):
# pick the next joint recording from the HDF5
def __init__(
self,
debug=False,
steps=120,
control_freq=CONTROL_FREQUENCY,
relative_action=True,
incremental_action=False,
action_scaling=1.0,
action_smoothing=1,
random_rot=(0, 0, 0),
reward_coefficients=(0.1, 1, 0),
stop_on_flip=False,
gait_factor=0.0,
):
""" Gym-compatible environment to teach the pupper how to walk
:param bool debug: If True, shows PyBullet in GUI mode. Set to False when training.
:param int steps: How long is the episode? Each step = one call to WalkingEnv.step()
:param int control_freq: How many simulation steps are there in a second of Pybullet sim. Pybullet always runs
at 240Hz but that's not optimal for RL control.
:param bool relative_action: If set to True, then all actions are added to the resting position.
This give the robot a more stable starting position.
"""
super().__init__()
# observation space:
# - 12 lef joints in the order
# - front right hip
# - front right upper leg
# - front right lower leg
# - front left hip/upper/lower leg
# - back right hip/upper/lower leg
# - back left hip/upper/lower leg
# - 3 body orientation in euler angles
# - 2 linear velocity (only along the plane, we don't care about z velocity
self.observation_space = spaces.Box(low=-1,
high=1,
shape=(12 + 3 + 2, ),
dtype=np.float32)
# action = 12 joint angles in the same order as above (fr/fl/br/bl and each with hip/upper/lower)
self.action_space = spaces.Box(low=-1,
high=1,
shape=(12, ),
dtype=np.float32)
# turning off start_standing because the that's done in self.reset()
kwargs = {}
# if relative_action: # if this is turned on, the robot is underpowered for the hardcoded gait
# kwargs = {"gain_pos": 1 / 16, "gain_vel": 1 / 8, "max_torque": 1 / 2}
self.sim = PupperSim2(debug=debug, start_standing=False, **kwargs)
self.episode_steps = 0
self.episode_steps_max = steps
self.control_freq = control_freq
self.dt = 1 / self.control_freq
self.incremental_angle = np.deg2rad(
MAX_ANGLE_PER_SEC) / self.control_freq
self.sim_steps = int(round(FREQ_SIM / control_freq))
self.relative_action = relative_action
self.incremental_action = incremental_action
self.action_scaling = action_scaling
self.action_smoothing = deque(maxlen=action_smoothing)
self.random_rot = random_rot
self.stop_on_flip = stop_on_flip
self.current_action = np.array([0] * 12)
self.gait = None
assert 0 <= gait_factor <= 1
self.gait_factor = gait_factor
self.joints_hard_limit_lower = (-np.pi + 0.001) * np.ones(12)
self.joints_hard_limit_uppper = (np.pi - 0.001) * np.ones(12)
# new reward coefficients
self.rcoeff_ctrl, self.rcoeff_run, self.rcoeff_stable = reward_coefficients
def __init__(self,
trick,
control_freq=CONTROL_FREQUENCY,
action_scaling=0.5):
super().__init__()
self.control_freq = control_freq
self.action_scaling = action_scaling
f = h5py.File(RECORDINGS_PATH.format(trick), "r")
self.vicon_positions = f["vicon_positions"]
self.vicon_rotations = f["vicon_rotations"]
self.idx_start = IMITATION_LIB[trick]["start"]
self.idx_end = IMITATION_LIB[trick]["end"]
self.base_pos = self.vicon_positions[self.idx_start]
self.base_rot = self.vicon_rotations[self.idx_end]
self.joints = f["joints"]
self.feet = f["foot_positions"]
# observation space:
# - 12 lef joints in the order
# - front right hip
# - front right upper leg
# - front right lower leg
# - front left hip/upper/lower leg
# - back right hip/upper/lower leg
# - back left hip/upper/lower leg
# - 3 body orientation in euler angles
# - 2 linear velocity (only along the plane, we don't care about z velocity
self.observation_space = gym.spaces.Box(low=-1,
high=1,
shape=(12 + 3 + 2, ),
dtype=np.float32)
# action = 12 joint angles in the same order as above (fr/fl/br/bl and each with hip/upper/lower)
self.action_space = gym.spaces.Box(low=-1,
high=1,
shape=(12, ),
dtype=np.float32)
self.sim_agent = PupperSim2(
debug=False,
start_standing=False,
gain_pos=1 / 16,
gain_vel=1 / 8,
max_torque=1 / 2,
img_size=(RESOLUTION, RESOLUTION),
)
self.sim_agent.change_color((SIM_AGENT_COLOR))
self.dt = 1 / self.control_freq
self.sim_steps = int(round(FREQ_SIM / control_freq))
self.sim_ref = PupperSim2(debug=False,
img_size=(RESOLUTION, RESOLUTION))
self.sim_ref.reset()
# make the robot limp for demonstration
self.sim_ref.make_kinematic(SIM_REF_COLOR)
self.episode_steps = 0
self.frame_idx = self.idx_start