def __call__(self, env, policy, debug=False, visualize=False):
episode_memory = queue()
observation = None
result = []
for episode in range(self.validate_episodes):
# reset at the start of episode
episode_memory.clear()
observation = env.reset()
episode_memory.append(observation)
observation = episode_memory.getObservation(self.window_length, observation, self.pic)
episode_steps = 0
episode_reward = 0.
assert observation is not None
# start episode
done = False
while not done and (episode_steps <= self.max_episode_length or not self.max_episode_length):
action = policy(observation)
observation, reward, done, info = env.step(action)
episode_memory.append(observation)
observation = episode_memory.getObservation(self.window_length, observation, self.pic)
if visualize:
if self.bullet:
import pybullet
pybullet.resetDebugVisualizerCamera(cameraDistance=10, cameraYaw=0, cameraPitch=-6.6,cameraTargetPosition=[10,0,0])
env.render()
episode_reward += reward
episode_steps += 1
result.append(episode_reward)
if debug: prRed('[Evaluate] reward:{}'.format(result))
return result
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=1,
isEnableSelfCollision=True,
renders=True):
print("init")
self._timeStep = 1./240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._width = 341
self._height = 256
self.terminated = 0
self._p = p
if self._renders:
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
else:
p.connect(p.DIRECT)
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
self._seed()
self.reset()
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
self.action_space = spaces.Discrete(7)
self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4))
self.viewer = None
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=1,
isEnableSelfCollision=True,
renders=False,
isDiscrete=False,
maxSteps = 1000):
#print("KukaGymEnv __init__")
self._isDiscrete = isDiscrete
self._timeStep = 1./240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._maxSteps = maxSteps
self.terminated = 0
self._cam_dist = 1.3
self._cam_yaw = 180
self._cam_pitch = -40
self._p = p
if self._renders:
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
else:
p.connect(p.DIRECT)
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
self.seed()
self.reset()
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([largeValObservation] * observationDim)
if (self._isDiscrete):
self.action_space = spaces.Discrete(7)
else:
action_dim = 3
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.viewer = None
def __init__(
self,
renderer='tiny', # ('tiny', 'egl', 'debug')
):
self.action_space = spaces.Discrete(2)
self.iter = cycle(range(0, 360, 10))
# how we want to show
assert renderer in ('tiny', 'egl', 'debug', 'plugin')
self._renderer = renderer
self._render_width = 84
self._render_height = 84
# connecting
if self._renderer == "tiny" or self._renderer == "plugin":
optionstring = '--width={} --height={}'.format(self._render_width, self._render_height)
p.connect(p.DIRECT, options=optionstring)
if self._renderer == "plugin":
plugin_fn = os.path.join(
p.__file__.split("bullet3")[0],
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
plugin = p.loadPlugin(plugin_fn, "_tinyRendererPlugin")
if plugin < 0:
print("\nPlugin Failed to load! Try installing via `pip install -e .`\n")
sys.exit()
print("plugin =", plugin)
elif self._renderer == "egl":
optionstring = '--width={} --height={}'.format(self._render_width, self._render_height)
optionstring += ' --window_backend=2 --render_device=0'
p.connect(p.GUI, options=optionstring)
elif self._renderer == "debug":
#print("Connection: SHARED_MEMORY")
#cid = p.connect(p.SHARED_MEMORY)
#if (cid<0):
cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
# -*- coding: utf-8 -*-
"""
Created on 2019
@author: Richard Bloemenkamp
"""
import pybullet as p
import time
import numpy as np
p.connect(p.GUI)
p.createCollisionShape(p.GEOM_PLANE)
plId=p.createMultiBody(0,0)
p.resetDebugVisualizerCamera( cameraDistance=4, cameraYaw=10, cameraPitch=-20,
cameraTargetPosition=[0.0, 0.0, 0.25])
sh_colFoot = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.01,0.01,0.1])
sh_colBody = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.1,0.1,0.1])
sh_colPx = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.01,0.1,0.1])
sh_colPy = p.createCollisionShape(p.GEOM_BOX,halfExtents=[0.1,0.01,0.1])
bodyId=p.createMultiBody(baseMass=0.8,baseCollisionShapeIndex = sh_colBody,
basePosition = [0,0,1.5],baseOrientation=[0,0,0,1])
footId=p.createMultiBody(baseMass=.1,baseCollisionShapeIndex = sh_colFoot,
basePosition = [0,0,0.5],baseOrientation=[0,0,0,1])
#4 inertia increasing plates
cubeId3=p.createMultiBody(baseMass=.1,baseCollisionShapeIndex = sh_colPx,
basePosition = [-0.5,0,1.5],baseOrientation=[0,0,0,1])
cubeId4=p.createMultiBody(baseMass=.1,baseCollisionShapeIndex = sh_colPx,
basePosition = [0.5,0,1.5],baseOrientation=[0,0,0,1])
cubeId5=p.createMultiBody(baseMass=.1,baseCollisionShapeIndex = sh_colPy,
basePosition = [0,-0.5,1.5],baseOrientation=[0,0,0,1])
try:
result = sys.stdin.read(1)
except IOError:
pass
finally:
termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm)
return result
p.connect(p.GUI)
# create the main plane
p.resetDebugVisualizerCamera(cameraDistance=5,
cameraYaw=90,
cameraPitch=-10,
cameraTargetPosition=[0, 0, 3])
p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, 0)
# create spheres and cubes
sphereRadius = 0.5
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
colBoxId = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[sphereRadius, sphereRadius, sphereRadius])
sphereMass = 2
cubeMass = 1
visualShapeId = -1 # not needed since we're using the collision shape ID, can set to -1
basePositionSphere = [0, 1, 5 * sphereRadius + 1]
basePositionCube = [0, -1, 5 * sphereRadius + 1]
return path
if __name__ == "__main__":
args = get_args()
# set up simulator
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(enableFileCaching=0)
p.setGravity(0, 0, -9.8)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, False)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, True)
p.resetDebugVisualizerCamera(
cameraDistance=1.400,
cameraYaw=58.000,
cameraPitch=-42.200,
cameraTargetPosition=(0.0, 0.0, 0.0),
)
# load objects
plane = p.loadURDF("plane.urdf")
ur5 = p.loadURDF("assets/ur5/ur5.urdf",
basePosition=[0, 0, 0.02],
useFixedBase=True)
obstacle1 = p.loadURDF("assets/block.urdf",
basePosition=[1 / 4, 0, 1 / 2],
useFixedBase=True)
obstacle2 = p.loadURDF("assets/block.urdf",
basePosition=[2 / 4, 0, 2 / 3],
useFixedBase=True)
obstacles = [plane, obstacle1, obstacle2]
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=80,
isEnableSelfCollision=True,
renders=False,
isDiscrete=True,
maxSteps=11,
dv=0.06,
removeHeightHack=False,
blockRandom=0.1,
cameraRandom=0,
width=64,
height=64,
numObjects=5,
isTest=False):
##############
self._width = IM_WIDTH
self._height = IM_HEIGHT
self.img_save_cnt = 0
self.N_DATA_TO_GENERATE = N_DATA_TO_GENERATE
self.N_UNSEEN_DATA_TO_GENERATE = N_UNSEEN_DATA_TO_GENERATE
##############
self._isDiscrete = isDiscrete
# self._timeStep = 1. / 240.
self._timeStep = 1. / 60.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._maxSteps = maxSteps
self.terminated = 0
self._cam_dist = 1.3
self._cam_yaw = 180
self._cam_pitch = -40
self._dv = dv
self._p = p
self._removeHeightHack = removeHeightHack
self._blockRandom = blockRandom
self._cameraRandom = cameraRandom
self._isTest = isTest
self.observation_space = spaces.Box(low=0,
high=255,
shape=(self._height, self._width, 3),
dtype=np.uint8)
#############################
self.model_paths, self.unseen_model_paths = self.get_data_path()
#############################
# disable GUI or not
if USE_GUI:
self.cid = p.connect(p.SHARED_MEMORY)
if (self.cid < 0):
self.cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33]) # cameraposition of rendering
else:
self.cid = p.connect(p.DIRECT)
self.seed()
if (self._isDiscrete):
if self._removeHeightHack:
self.action_space = spaces.Discrete(9)
else:
self.action_space = spaces.Discrete(7)
else:
self.action_space = spaces.Box(low=-1, high=1, shape=(3,)) # dx, dy, da
if self._removeHeightHack:
self.action_space = spaces.Box(low=-1, high=1, shape=(4,)) # dx, dy, dz, da
self.viewer = None
import pybullet as p
import time
import pybullet_data
physicsClient = p.connect(p.GUI) # or p.DIRECT for non graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
#p.setTimeStep(1.0)
planeID = p.loadURDF("plane.urdf")
p.resetDebugVisualizerCamera(cameraDistance=0.5,
cameraYaw=75,
cameraPitch=-20,
cameraTargetPosition=[0.0, 0.0, 0.25])
baseStartPos = [0, 0, 0.5]
baseStartOrientation = p.getQuaternionFromEuler([1, 0, 0])
quadrupedID = p.loadURDF("MQ_SIM_ASEM/urdf/MQ_SIM_ASEM.urdf", baseStartPos,
baseStartOrientation)
#p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
# Set Joints to home position
for joint in range(p.getNumJoints(quadrupedID)):
p.setJointMotorControl2(quadrupedID,
joint,
p.POSITION_CONTROL,
targetPosition=0.0,
force=10,
maxVelocity=5)
# Set Shoulder Joints
joint = 0
def reset(self):
super(DrinkingEnv, self).reset()
self.build_assistive_env('wheelchair')
if self.robot.wheelchair_mounted:
wheelchair_pos, wheelchair_orient = self.furniture.get_base_pos_orient(
)
self.robot.set_base_pos_orient(
wheelchair_pos +
np.array(self.robot.toc_base_pos_offset[self.task]),
[0, 0, -np.pi / 2.0])
# Update robot and human motor gains
self.robot.motor_gains = self.human.motor_gains = 0.005
joints_positions = [(self.human.j_right_elbow, -90),
(self.human.j_left_elbow, -90),
(self.human.j_right_hip_x, -90),
(self.human.j_right_knee, 80),
(self.human.j_left_hip_x, -90),
(self.human.j_left_knee, 80)]
joints_positions += [
(self.human.j_head_x, self.np_random.uniform(-30, 30)),
(self.human.j_head_y, self.np_random.uniform(-30, 30)),
(self.human.j_head_z, self.np_random.uniform(-30, 30))
]
self.human.setup_joints(joints_positions,
use_static_joints=True,
reactive_force=None)
self.generate_target()
p.resetDebugVisualizerCamera(cameraDistance=1.10,
cameraYaw=55,
cameraPitch=-45,
cameraTargetPosition=[-0.2, 0, 0.75],
physicsClientId=self.id)
# Initialize the tool in the robot's gripper
self.tool.init(self.robot,
self.task,
self.directory,
self.id,
self.np_random,
right=True,
mesh_scale=[0.045] * 3,
alpha=0.75)
self.cup_top_center_offset = np.array([0, 0, -0.055])
self.cup_bottom_center_offset = np.array([0, 0, 0.07])
target_ee_pos = np.array([-0.2, -0.5, 1]) + self.np_random.uniform(
-0.05, 0.05, size=3)
target_ee_orient = self.get_quaternion(
self.robot.toc_ee_orient_rpy[self.task])
self.init_robot_pose(target_ee_pos,
target_ee_orient,
[(target_ee_pos, target_ee_orient),
(self.target_pos, None)],
[(self.target_pos, target_ee_orient)],
arm='right',
tools=[self.tool],
collision_objects=[self.human, self.furniture])
# Open gripper to hold the tool
self.robot.set_gripper_open_position(self.robot.right_gripper_indices,
self.robot.gripper_pos[self.task],
set_instantly=True)
if not self.robot.mobile:
self.robot.set_gravity(0, 0, 0)
self.human.set_gravity(0, 0, 0)
self.tool.set_gravity(0, 0, 0)
p.setPhysicsEngineParameter(numSubSteps=4,
numSolverIterations=10,
physicsClientId=self.id)
# Generate water
cup_pos, cup_orient = self.tool.get_base_pos_orient()
water_radius = 0.005
water_mass = 0.001
batch_positions = []
for i in range(4):
for j in range(4):
for k in range(4):
batch_positions.append(
np.array([
i * 2 * water_radius - 0.02, j * 2 * water_radius -
0.02, k * 2 * water_radius + 0.075
]) + cup_pos)
self.waters = self.create_spheres(radius=water_radius,
mass=water_mass,
batch_positions=batch_positions,
visual=False,
collision=True)
for w in self.waters:
p.changeVisualShape(w.body,
-1,
rgbaColor=[0.25, 0.5, 1, 1],
physicsClientId=self.id)
self.total_water_count = len(self.waters)
self.waters_active = [w for w in self.waters]
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
# Drop water in the cup
for _ in range(50):
p.stepSimulation(physicsClientId=self.id)
self.init_env_variables()
return self._get_obs()
import pybullet_data
import os
import time
# physics parameter setting
fixedTimeStep = 1. / 1000
numSolverIterations = 200
physicsClient = p.connect(p.GUI)
p.setTimeStep(fixedTimeStep)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # to load plane.urdf
p.setGravity(0, 0, -9.81)
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=10,
cameraPitch=-5,
cameraTargetPosition=[0.3, 0.5, 0.1])
StartPos = [0, 0, 0.3]
StartOrientation = p.getQuaternionFromEuler([0, 0, 0])
# samurai.urdf plane.urdf
planeId = p.loadURDF("plane.urdf")
robot = p.loadURDF("test-1.urdf", StartPos, StartOrientation)
p.setRealTimeSimulation(1) # real-time instead of step control
while 1:
# p.stepSimulation()
# time.sleep(0.01)
block_config[block] = (pos, quat, colors[b])
block_id = {}
for blk, (pos, quat, rgb) in block_config.items():
b = pb.loadURDF('cube.urdf',
basePosition=pos,
baseOrientation=quat,
useFixedBase=False)
pb.changeVisualShape(b, linkIndex=-1, rgbaColor=rgb + (1, ))
block_id[blk] = b
env.step() # let blocks settle
# from check/camera.py
pb.resetDebugVisualizerCamera(
1.2000000476837158, 56.799964904785156, -22.20000648498535,
(-0.6051651835441589, 0.26229506731033325, -0.24448847770690918))
def get_tip_targets(p, q, d):
m = pb.getMatrixFromQuaternion(q)
t1 = p[0] - d * m[1], p[1] - d * m[4], p[2] - d * m[7]
t2 = p[0] + d * m[1], p[1] + d * m[4], p[2] + d * m[7]
return (t1, t2)
action = [0.] * env.num_joints
# env.set_position(action)
env.goto_position(action, 1)
##########################################
t_stance = 0.2
### Initialization
steps = int(t_stance / dt)
t = 0.0
# Video logger settings
log_video = False
if log_video:
video_name = "./video/small model/trial5.mp4"
fps = int(1 / dt)
if fps > 480:
fps = 480
writer = imageio.get_writer(video_name, fps=fps)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(cameraDistance=0.1,
cameraYaw=50,
cameraPitch=-60,
cameraTargetPosition=[0, 0, 0])
# Data logger settings
log_data = True
eCOM_history = np.array([0])
eq_history = np.array([0] * model.state.nqd)
des_history = list()
tau_history = list()
state_history = list()
# Starting position and legs abduction range
theta_g0 = pi / 6
theta_gf = -pi / 2
# p.resetJointState(model.Id, model.control_indices[1][0],theta_g0)
# p.resetJointState(model.Id, model.control_indices[2][0],-theta_gf)
model.set_bent_position(theta_rg=theta_g0,
theta_lg=-theta_gf,
timeStep = 1. / fps
if createVideo:
p.connect(
p.GUI,
options=
"--minGraphicsUpdateTimeMs=0 --mp4=\"pybullet_grasp.mp4\" --mp4fps=" +
str(fps))
else:
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setPhysicsEngineParameter(maxNumCmdPer1ms=1000)
p.resetDebugVisualizerCamera(cameraDistance=1.3,
cameraYaw=38,
cameraPitch=-22,
cameraTargetPosition=[0.35, -0.13, 0])
p.setAdditionalSearchPath(pd.getDataPath())
p.setTimeStep(timeStep)
p.setGravity(0, -9.8, 0)
panda = panda_sim.PandaSimAuto(p, [0, 0, 0])
panda.control_dt = timeStep
logId = panda.bullet_client.startStateLogging(
panda.bullet_client.STATE_LOGGING_PROFILE_TIMINGS, "log.json")
panda.bullet_client.submitProfileTiming("start")
for i in range(100000):
panda.bullet_client.submitProfileTiming("full_step")
panda.step()
jointPoses : [float] * numDofs
"""
numJoints = p.getNumJoints(bodyId)
for i in range(numJoints):
jointInfo = p.getJointInfo(bodyId, i)
qIndex = jointInfo[3]
if qIndex > -1:
p.setJointMotorControl2(bodyIndex=bodyId, jointIndex=i, controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[qIndex-7])
if __name__ == "__main__":
guiClient = p.connect(p.GUI)
p.resetDebugVisualizerCamera(2., 180, 0., [0.52, 0.2, np.pi/4.])
baxterId, endEffectorId = setUpWorld()
lowerLimits, upperLimits, jointRanges, restPoses = getJointRanges(baxterId, includeFixed=False)
#targetPosition = [0.2, 0.8, -0.1]
#targetPosition = [0.8, 0.2, -0.1]
targetPosition = [0.2, 0.0, -0.1]
p.addUserDebugText("TARGET", targetPosition, textColorRGB=[1,0,0], textSize=1.5)
maxIters = 100000
import time
p.connect(p.GUI)
#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG, globalCFM = 0.000001)
#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
p.loadURDF("plane.urdf")
radius = 0.025
distance = 1.86
yaw=135
pitch=-11
targetPos=[0,0,0]
p.setPhysicsEngineParameter(solverResidualThreshold=0.001, numSolverIterations=200)
p.resetDebugVisualizerCamera(distance, yaw,pitch, targetPos)
objectId = -1
for i in range (10):
objectId = p.loadURDF("cube_small.urdf",[1,1,radius+i*2*radius])
p.changeDynamics(objectId,-1,100)
timeStep = 1./240.
p.setGravity(0,0,-10)
while (p.isConnected()):
p.stepSimulation()
time.sleep(timeStep)
def __init__(self,
urdf_path=URDF_PATH,
max_steps=250,
env_rank=0,
random_target=False,
srl_pipe=None,
is_discrete=True,
action_repeat=1,
srl_model="ground_truth",
control_mode="position",
seed=0,
debug_mode=False,
**_):
super(JakaButtonObsGymEnv, self).__init__(srl_model=srl_model,
relative_pos=True,
env_rank=env_rank,
srl_pipe=srl_pipe)
self.seed(seed)
self.urdf_path = urdf_path
self._random_target = random_target
self._observation = None
self.debug_mode = debug_mode
self._inmoov = None
self._observation = None
self.action_repeat = action_repeat
self._jaka_id = -1
self._button_id = -1
self.max_steps = max_steps
self._step_counter = 0
self._render = False
self.position_control = (control_mode == "position")
self.discrete_action = is_discrete
self.srl_model = srl_model
self.camera_target_pos = (0.0, 0.0, 1.0)
self._width = RENDER_WIDTH
self._height = RENDER_HEIGHT
self.terminated = False
self.n_contacts = 0
self.state_dim = self.getGroundTruthDim()
self._first_reset_flag = False
if debug_mode:
client_id = p.connect(p.SHARED_MEMORY)
if client_id < 0:
p.connect(p.GUI)
p.resetDebugVisualizerCamera(2., 180, -41, [0., -0.1, 0.1])
else:
p.connect(p.DIRECT)
self.action_space = spaces.Discrete(6)
if self.srl_model == "raw_pixels":
self.observation_space = spaces.Box(low=0,
high=255,
shape=(self._height,
self._width, 3),
dtype=np.uint8)
else: # Todo: the only possible observation for srl_model is ground truth
self.observation_space = spaces.Box(low=-np.inf,
high=np.inf,
shape=(3, ),
dtype=np.float32)
self.reset()
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=1, # <---- was 8?
isEnableSelfCollision=True,
renders=False,
isDiscrete=False,
maxSteps=1000, # <---- was 8?
dv=0.06,
removeHeightHack=True,
blockRandom=0.3,
cameraRandom=0,
width=48,
height=48,
numObjects=3,
isTest=False):
"""Initializes the KukaDiverseObjectEnv.
Args:
urdfRoot: The diretory from which to load environment URDF's.
actionRepeat: The number of simulation steps to apply for each action.
isEnableSelfCollision: If true, enable self-collision.
renders: If true, render the bullet GUI.
isDiscrete: If true, the action space is discrete. If False, the
action space is continuous.
maxSteps: The maximum number of actions per episode.
dv: The velocity along each dimension for each action.
removeHeightHack: If false, there is a "height hack" where the gripper
automatically moves down for each action. If true, the environment is
harder and the policy chooses the height displacement.
blockRandom: A float between 0 and 1 indicated block randomness. 0 is
deterministic.
cameraRandom: A float between 0 and 1 indicating camera placement
randomness. 0 is deterministic.
width: The image width.
height: The observation image height.
numObjects: The number of objects in the bin.
isTest: If true, use the test set of objects. If false, use the train
set of objects.
"""
self._isDiscrete = isDiscrete
self._timeStep = 1. / 240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._maxSteps = maxSteps
self.terminated = 0
self._cam_dist = 1.3
self._cam_yaw = 180
self._cam_pitch = -40
self._dv = dv
self._p = p
self._removeHeightHack = removeHeightHack
self._blockRandom = blockRandom
self._cameraRandom = cameraRandom
self._width = width
self._height = height
self._numObjects = numObjects
self._isTest = isTest
if self._renders:
self.cid = p.connect(p.SHARED_MEMORY)
if (self.cid < 0):
self.cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
else:
self.cid = p.connect(p.DIRECT)
self._seed()
if (self._isDiscrete):
if self._removeHeightHack:
self.action_space = spaces.Discrete(9)
else:
self.action_space = spaces.Discrete(7)
else:
self.action_space = spaces.Box(low=-1, high=1, shape=(3,)) # dx, dy, da
if self._removeHeightHack:
self.action_space = spaces.Box(low=-1,
high=1,
shape=(4,)) # dx, dy, dz, da
# self.observation_space = spaces.Box(-observation_high, observation_high) <--- is absent
self.viewer = None
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=80,
isEnableSelfCollision=True,
renders=False,
isDiscrete=False,
maxSteps=8,
dv=0.06,
removeHeightHack=False,
blockRandom=0.3,
cameraRandom=0,
width=48,
height=48,
numObjects=5,
isTest=False):
"""Initializes the KukaDiverseObjectEnv.
Args:
urdfRoot: The diretory from which to load environment URDF's.
actionRepeat: The number of simulation steps to apply for each action.
isEnableSelfCollision: If true, enable self-collision.
renders: If true, render the bullet GUI.
isDiscrete: If true, the action space is discrete. If False, the
action space is continuous.
maxSteps: The maximum number of actions per episode.
dv: The velocity along each dimension for each action.
removeHeightHack: If false, there is a "height hack" where the gripper
automatically moves down for each action. If true, the environment is
harder and the policy chooses the height displacement.
blockRandom: A float between 0 and 1 indicated block randomness. 0 is
deterministic.
cameraRandom: A float between 0 and 1 indicating camera placement
randomness. 0 is deterministic.
width: The image width.
height: The observation image height.
numObjects: The number of objects in the bin.
isTest: If true, use the test set of objects. If false, use the train
set of objects.
"""
self._isDiscrete = isDiscrete
self._timeStep = 1./240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._maxSteps = maxSteps
self.terminated = 0
self._cam_dist = 1.3
self._cam_yaw = 180
self._cam_pitch = -40
self._dv = dv
self._p = p
self._removeHeightHack = removeHeightHack
self._blockRandom = blockRandom
self._cameraRandom = cameraRandom
self._width = width
self._height = height
self._numObjects = numObjects
self._isTest = isTest
if self._renders:
self.cid = p.connect(p.SHARED_MEMORY)
if (self.cid<0):
self.cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
else:
self.cid = p.connect(p.DIRECT)
self._seed()
if (self._isDiscrete):
if self._removeHeightHack:
self.action_space = spaces.Discrete(9)
else:
self.action_space = spaces.Discrete(7)
else:
self.action_space = spaces.Box(low=-1, high=1, shape=(3,)) # dx, dy, da
if self._removeHeightHack:
self.action_space = spaces.Box(low=-1,
high=1,
shape=(4,)) # dx, dy, dz, da
self.viewer = None
# Get infos on end effector and gripper link
# end effector link = 'lbr_iiwa_link_7', gripper link = 'base_link'
kukaEndEffectorIndex = 6
kukaGripperIndex = 7
jointInfoEndEffector = p.getJointInfo(kukaUid, kukaEndEffectorIndex)
linkNameEndEffector = str(jointInfoEndEffector[12].decode("utf-8"))
jointInfoGripper = p.getJointInfo(kukaUid, kukaGripperIndex)
linkNameGripper = str(jointInfoGripper[12].decode("utf-8"))
print("-> link : end effector={0}, gripper={1}".format(linkNameEndEffector,
linkNameGripper))
# reset the initial view of the environment (to be closer to robot)
p.resetDebugVisualizerCamera(cameraDistance=1.5,
cameraYaw=0,
cameraPitch=-40,
cameraTargetPosition=[0.35, 0.0, 0.7])
# reset the position and orientation of the base (root) of the robot kuka
p.resetBasePositionAndOrientation(kukaUid, [-0.100000, 0.000000, 0.070000],
[0.000000, 0.000000, 0.000000, 1.000000])
#Add sliders for all joints (useful to move the joints)
joint_name_to_ids = {} # dictionary to associate a joint name to index
joint_name_to_slider = {} # dictionary to assciate a joint name to a slider
# Put kuka robot in an 'initial position' for grasping
initial_positions = [
0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539
]
p.resetJointState(robot, joint_id["leftAnklePitch"], -0.6, 0.)
p.resetJointState(robot, joint_id["rightAnklePitch"], -0.6, 0.)
p.resetJointState(robot, joint_id["rightShoulderPitch"], 0.2, 0.)
p.resetJointState(robot, joint_id["leftShoulderPitch"], 0.2, 0.)
p.resetJointState(robot, joint_id["leftShoulderRoll"], -1.1, 0.)
p.resetJointState(robot, joint_id["rightShoulderRoll"], 1.1, 0.)
p.resetJointState(robot, joint_id["rightElbowPitch"], 1.57, 0.)
p.resetJointState(robot, joint_id["leftElbowPitch"], -1.57, 0.)
if __name__ == "__main__":
# Environment Setup
p.connect(p.GUI)
p.resetDebugVisualizerCamera(cameraDistance=1.5,
cameraYaw=120,
cameraPitch=-30,
cameraTargetPosition=[1, 0.5, 1.5])
p.setGravity(0, 0, -9.81)
p.setPhysicsEngineParameter(fixedTimeStep=DT, numSubSteps=1)
if VIDEO_RECORD:
if not os.path.exists('video'):
os.makedirs('video')
for f in os.listdir('video'):
if f == 'valkyrie_kin.mp4':
os.remove('video/' + f)
p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4,
"video/valkyrie_kin.mp4")
# Create Robot, Ground
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
robot = p.loadURDF(cwd + "/robot_model/valkyrie/valkyrie.urdf",
"/home/h3ct0r/catkin_ws_espeleo/src/espeleo_planner/espeleo_planner/test/maps/map_01_frontiers.stl",
rgbaColor=(0.6, 0.3, 0.1, 0.7))
body_id = p.createMultiBody(
baseCollisionShapeIndex=col_shape_id,
baseVisualShapeIndex=viz_shape_id,
basePosition=(0, 0, 0),
baseOrientation=(0, 0, 0, 1),
)
cam_dist = 2.60
cam_yaw = 97.60
cam_pitch = -6.20
cam_pos = [-0.03, 0.24, 0]
p.resetDebugVisualizerCamera(cameraDistance=cam_dist,
cameraYaw=cam_yaw,
cameraPitch=cam_pitch,
cameraTargetPosition=cam_pos)
def estimate_position(startPos, startOrientation):
start_time = datetime.datetime.now()
startQuatOrientation = p.getQuaternionFromEuler(startOrientation)
# boxId = p.loadURDF("/home/h3ct0r/catkin_ws_espeleo/src/espeleo_planner/espeleo_planner/urdf/espeleo2_low_poly.urdf",startPos, startQuatOrientation)
# linkId = 0
boxId = p.loadURDF(
"/home/h3ct0r/catkin_ws_espeleo/src/espeleo_planner/espeleo_planner/urdf/espeleo2_low_poly_prismatic.urdf",
startPos, startQuatOrientation)
linkId = 1
import pybullet as p
from time import sleep
import pybullet_data
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
p.resetDebugVisualizerCamera(3, -420, -30, [0.3, 0.9, -2])
p.setGravity(0, 0, -10)
tex = p.loadTexture("uvmap.png")
planeId = p.loadURDF("plane.urdf", [0, 0, -2])
boxId = p.loadURDF("cube.urdf", [0, 3, 2], useMaximalCoordinates=True)
bunnyId = p.loadSoftBody("torus/torus_textured.obj",
simFileName="torus.vtk",
mass=3,
useNeoHookean=1,
NeoHookeanMu=180,
NeoHookeanLambda=600,
NeoHookeanDamping=0.01,
collisionMargin=0.006,
useSelfCollision=1,
frictionCoeff=0.5,
repulsionStiffness=800)
p.changeVisualShape(bunnyId,
-1,
rgbaColor=[1, 1, 1, 1],
import os
import pybullet as p
import time
import pybullet_data
import numpy as np
from gym_ergojr import get_scene
from gym_ergojr.models import MODEL_PATH
from gym_ergojr.utils.pybullet import DistanceBetweenObjects
from gym_ergojr.utils.urdf_helper import URDF
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
p.resetDebugVisualizerCamera(cameraDistance=0.4,
cameraYaw=135,
cameraPitch=-35,
cameraTargetPosition=[0, 0.05, 0])
p.setGravity(0, 0, -10) # good enough
frequency = 100 # Hz
p.setTimeStep(1 / frequency)
p.setRealTimeSimulation(0)
planeId = p.loadURDF(URDF(get_scene("plane-big.urdf.xml")).get_path())
startPos = [0, 0, 0] # xyz
startOrientation = p.getQuaternionFromEuler(
[0, 0, 0]) # rotated around which axis? # np.deg2rad(90)
xml_path = get_scene("ergojr-pusher")
robot_file = URDF(xml_path, force_recompile=True).get_path()
p.setRealTimeSimulation(1)
else:
fastForward = True
p.setRealTimeSimulation(0)
elif nkey in keys and keys[nkey] & p.KEY_WAS_TRIGGERED:
p.resetSimulation()
p.setRealTimeSimulation(0)
shape = util.nextGen(nbobj, rad, pos1, shape)
p.setRealTimeSimulation(1)
maxFit = 0
continue
posbody = p.getBasePositionAndOrientation(shape[0].getId())[0]
curCam = p.getDebugVisualizerCamera()
p.resetDebugVisualizerCamera(cameraDistance=curCam[10],
cameraYaw=curCam[8],
cameraPitch=curCam[9],
cameraTargetPosition=posbody)
curx = posbody[0]
cury = posbody[1]
curz = posbody[2]
fitness = (np.abs(curx - basex) + np.abs(cury - basey))
if maxFit < fitness:
maxFit = fitness
# print(maxFit)
if fastForward:
p.stepSimulation()
# time.sleep(1/1000)
pcl_node = PointCloudPublisher()
pcl_node.run_thread()
br = tf.TransformBroadcaster()
##########################################################
model_dir = os.getcwd() # Hack
pcid = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0, physicsClientId=pcid)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,
0,
physicsClientId=pcid)
p.resetDebugVisualizerCamera(cameraDistance=1.2,
cameraYaw=90,
cameraPitch=-15,
cameraTargetPosition=[0.5, -0.35, 0.2],
physicsClientId=pcid)
p.resetSimulation(physicsClientId=pcid)
p.setGravity(0, 0, -9.81, physicsClientId=pcid)
p.setPhysicsEngineParameter(numSolverIterations=900, physicsClientId=pcid)
time_step = 1. / 200.
p.setTimeStep(time_step, physicsClientId=pcid)
p.loadURDF(model_dir + '/models/table/table.urdf', [0.5, -0.4, -0.075],
useFixedBase=True,
physicsClientId=pcid)
robot = p.loadURDF(model_dir + '/models/kinova/kinova.urdf',
useFixedBase=True,
while True:
with torch.no_grad():
value, action, _, recurrent_hidden_states = actor_critic.act(
obs,
recurrent_hidden_states,
masks,
deterministic=args.det,
player_act=player_act)
if num_step >= 5000:
env.reset()
num_step = 0
# Obser reward and next obs
obs, reward, done, infos = env.step(action)
player_act = None
if infos[0]:
if 'player_move' in infos[0].keys():
player_act = infos[0]['player_move']
num_step += 1
masks.fill_(0.0 if done else 1.0)
if args.env_name.find('Bullet') > -1:
if torsoId > -1:
distance = 5
yaw = 0
humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId)
p.resetDebugVisualizerCamera(distance, yaw, -20, humanPos)
def precompute_environment_costs(numEnvs, K, L, params, husky, sphere, GUI,
seed, environment, obsUid):
# Parameters
numRays = params['numRays']
senseRadius = params['senseRadius']
robotRadius = params['robotRadius']
robotHeight = params['robotHeight']
thetas_nominal = params['thetas_nominal']
T_horizon = params['T_horizon']
# Fix random seed for consistency of results
np.random.seed(seed)
# Initialize costs for the different environments and different controllers
costs = np.zeros((numEnvs, L))
for env in range(0, numEnvs):
# Print
if (env % 10 == 0):
print(env, "out of", numEnvs)
# Sample environment
# heightObs = 20*robotHeight
# obsUid = generate_obstacles(pybullet, heightObs, robotRadius)
for l in range(0, L):
# Initialize position of robot
state = [0.0, 1.0, 0.0] # [x, y, theta]
quat = pybullet.getQuaternionFromEuler([
0.0, 0.0, state[2] + np.pi / 2
]) # pi/2 since Husky visualization is rotated by pi/2
pybullet.resetBasePositionAndOrientation(husky,
[state[0], state[1], 0.0],
quat)
pybullet.resetBasePositionAndOrientation(
sphere, [state[0], state[1], robotHeight], [0, 0, 0, 1])
# Cost for this particular controller (lth controller) in this environment
cost_env_l = 0.0
all_angles = []
for t in range(0, T_horizon):
# Get sensor measurement
y = getDistances(pybullet, state, robotHeight, numRays,
senseRadius, thetas_nominal)
# Compute control input
# u = compute_control(y, K[l])
angle = compute_fgm_angle(y, thetas_nominal, state[2])
all_angles.append(angle)
# Update state
# state = robot_update_state(state, u)
state, cost_env_l, done, _ = environment.step_fgm(angle)
# Update position of pybullet object
# quat = pybullet.getQuaternionFromEuler([0.0, 0.0, state[2]+np.pi/2]) # pi/2 since Husky visualization is rotated by pi/2
# pybullet.resetBasePositionAndOrientation(husky, [state[0], state[1], 0.0], quat)
# pybullet.resetBasePositionAndOrientation(sphere, [state[0], state[1], robotHeight], [0,0,0,1])
if (GUI):
pybullet.resetDebugVisualizerCamera(cameraDistance=5.0,
cameraYaw=0.0,
cameraPitch=-45.0,
cameraTargetPosition=[
state[0], state[1],
2 * robotHeight
])
time.sleep(0.025)
# Check if the robot is in collision. If so, cost = 1.0.
# Get closest points. Note: Last argument is distance threshold. Since it's set to 0, the function will only return points if the distance is less than zero. So, closestPoints is non-empty iff there is a collision.
# closestPoints = pybullet.getClosestPoints(sphere, obsUid, 0.0)
# See if the robot is in collision. If so, cost = 1.0.
'''
if closestPoints: # Check if closestPoints is non-empty
cost_env_l = 1.0
break # break out of simulation for this environment
'''
if cost_env_l == 1.0:
break
# Check that cost is between 0 and 1 (for sanity)
if (cost_env_l > 1.0):
raise ValueError("Cost is greater than 1!")
if (cost_env_l < 0.0):
raise ValueError("Cost is less than 0!")
# Record cost for this environment and this controller
costs[env][l] = cost_env_l
# Remove obstacles
pybullet.removeBody(obsUid)
print("mean angle = " + str(sum(all_angles) / len(all_angles)))
return costs
controlMode=p.TORQUE_CONTROL,
force=-torque)
# print("Torque:",torque)
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
p.setGravity(0, 0, -10)
sleep(0.01) # Time in seconds.
else:
# p.setJointMotorControl2(bodyUniqueId=boxId,jointIndex=0, controlMode=p.VELOCITY_CONTROL, targetVelocity = 1, force = 500)
robotPos = p.getLinkState(boxId, 3)[0]
p.resetDebugVisualizerCamera(5, 0, -20, robotPos)
footLength = p.getJointState(boxId, 4)[0]
# print("JointInfo:",footLength) #Get foot joint info
springLength = abs(float(1) - abs(footLength))
legLength = springLength + 0.5
legLengthDifference = springLength - currentSpringLength
currentSpringLength = springLength
if abs(legLengthDifference) < 0.000001: legLengthDifference = 0
# print("legLengthDifference:",legLengthDifference)
stiffness = float(1) / springLength
springHardness = 5000.0
#print("stiffness:", stiffness)
legTorque = stiffness * float(abs(1 - springLength)) * springHardness
legAndBodyAngle = 1.57079632679 - p.getJointState(boxId, 3)[0]
#print("legTorque:", legTorque)
toeLinkHeight = p.getLinkState(boxId, 5)[0][2]
def init_simulator():
global boxId, reset, low_energy_mode, high_performance_mode, terrain
robot_start_pos = [0, 0, 0.42]
p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
reset = p.addUserDebugParameter("reset", 1, 0, 0)
low_energy_mode = p.addUserDebugParameter("low_energy_mode", 1, 0, 0)
high_performance_mode = p.addUserDebugParameter("high_performance_mode", 1, 0, 0)
p.setGravity(0, 0, -9.8)
p.setTimeStep(1.0/freq)
p.resetDebugVisualizerCamera(0.2, 45, -30, [1, -1, 1])
# p.setPhysicsEngineParameter(numSolverIterations=30)
# p.setPhysicsEngineParameter(enableConeFriction=0)
# planeId = p.loadURDF("plane.urdf")
# p.changeDynamics(planeId, -1, lateralFriction=1.0)
# boxId = p.loadURDF("/home/wgx/Workspace_Ros/src/cloudrobot/src/quadruped_robot.urdf", robot_start_pos,
# useFixedBase=FixedBase)
heightPerturbationRange = 0.06
numHeightfieldRows = 256
numHeightfieldColumns = 256
if terrain == "plane":
planeShape = p.createCollisionShape(shapeType=p.GEOM_PLANE)
ground_id = p.createMultiBody(0, planeShape)
p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0, 0, 1])
elif terrain == "random1":
heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns
for j in range(int(numHeightfieldColumns/2)):
for i in range(int(numHeightfieldRows/2)):
height = random.uniform(0, heightPerturbationRange)
heightfieldData[2*i+2*j*numHeightfieldRows] = height
heightfieldData[2*i+1+2*j*numHeightfieldRows] = height
heightfieldData[2*i+(2*j+1)*numHeightfieldRows] = height
heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows] = height
terrainShape = p.createCollisionShape(shapeType=p.GEOM_HEIGHTFIELD, meshScale=[.05, .05, 1], heightfieldTextureScaling=(
numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns)
ground_id = p.createMultiBody(0, terrainShape)
p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0, 0, 1])
elif terrain == "random2":
terrain_shape = p.createCollisionShape(
shapeType=p.GEOM_HEIGHTFIELD,
meshScale=[.5, .5, .5],
fileName="heightmaps/ground0.txt",
heightfieldTextureScaling=128)
ground_id = p.createMultiBody(0, terrain_shape)
path = rospack.get_path('quadruped_ctrl')
textureId = p.loadTexture(path+"/model/grass.png")
p.changeVisualShape(ground_id, -1, textureUniqueId=textureId)
p.resetBasePositionAndOrientation(ground_id, [1, 0, 0.2], [0, 0, 0, 1])
elif terrain == "stairs":
planeShape = p.createCollisionShape(shapeType=p.GEOM_PLANE)
ground_id = p.createMultiBody(0, planeShape)
# p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0.0872, 0, 0.9962])
p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0, 0, 1])
# many box
colSphereId = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.01])
colSphereId1 = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.02])
colSphereId2 = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.03])
colSphereId3 = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.04])
# colSphereId4 = p.createCollisionShape(
# p.GEOM_BOX, halfExtents=[0.03, 0.03, 0.03])
p.createMultiBody(100, colSphereId, basePosition=[1.0, 1.0, 0.0])
p.changeDynamics(colSphereId, -1, lateralFriction=lateralFriction)
p.createMultiBody(100, colSphereId1, basePosition=[1.2, 1.0, 0.0])
p.changeDynamics(colSphereId1, -1, lateralFriction=lateralFriction)
p.createMultiBody(100, colSphereId2, basePosition=[1.4, 1.0, 0.0])
p.changeDynamics(colSphereId2, -1, lateralFriction=lateralFriction)
p.createMultiBody(100, colSphereId3, basePosition=[1.6, 1.0, 0.0])
p.changeDynamics(colSphereId3, -1, lateralFriction=lateralFriction)
# p.createMultiBody(10, colSphereId4, basePosition=[2.7, 1.0, 0.0])
# p.changeDynamics(colSphereId4, -1, lateralFriction=0.5)
p.changeDynamics(ground_id, -1, lateralFriction=lateralFriction)
boxId = p.loadURDF("mini_cheetah/mini_cheetah.urdf", robot_start_pos,
useFixedBase=False)
p.changeDynamics(boxId, 3, spinningFriction=spinningFriction)
p.changeDynamics(boxId, 7, spinningFriction=spinningFriction)
p.changeDynamics(boxId, 11, spinningFriction=spinningFriction)
p.changeDynamics(boxId, 15, spinningFriction=spinningFriction)
jointIds = []
for j in range(p.getNumJoints(boxId)):
p.getJointInfo(boxId, j)
jointIds.append(j)
# slope terrain
# colSphereId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.5, 0.5, 0.001])
# colSphereId1 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.5, 0.06])
# BoxId = p.createMultiBody(100, colSphereId, basePosition=[1.0, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId1, basePosition=[1.6, 1.0, 0.0])
# stairs
# colSphereId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.02])
# colSphereId1 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.04])
# colSphereId2 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.06])
# colSphereId3 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.08])
# colSphereId4 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[1.0, 0.4, 0.1])
# BoxId = p.createMultiBody(100, colSphereId, basePosition=[1.0, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId1, basePosition=[1.2, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId2, basePosition=[1.4, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId3, basePosition=[1.6, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId4, basePosition=[2.7, 1.0, 0.0])
reset_robot()
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.25])
minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True)
print(p.getNumJoints(minitaur))
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=23.2,
cameraPitch=-6.6,
cameraTargetPosition=[-0.064, .621, -0.2])
motorJointId = 1
p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0)
p.resetJointState(minitaur, motorJointId, targetValue=0, targetVelocity=1)
angularDampingSlider = p.addUserDebugParameter("angularDamping", 0, 1, 0)
jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0, 0.1, 0)
textId = p.addUserDebugText("jointVelocity=0", [0, 0, -0.2])
p.setRealTimeSimulation(1)
while (1):
frictionForce = p.readUserDebugParameter(jointFrictionForceSlider)
angularDamping = p.readUserDebugParameter(angularDampingSlider)
p.setJointMotorControl2(minitaur,
motorJointId,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=frictionForce)
p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping)
time.sleep(0.01)
def __init__(self,
urdfRoot=robot_data.getDataPath(),
actionRepeat=4,
useIK=1,
isDiscrete=0,
control_arm='l',
useOrientation=0,
rnd_obj_pose=1,
renders=False,
maxSteps=1000):
self._control_arm = control_arm
self._isDiscrete = isDiscrete
self._timeStep = 1. / 240.
self._useIK = 1 if self._isDiscrete else useIK
self._useOrientation = useOrientation
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._maxSteps = maxSteps
self.terminated = 0
self._cam_dist = 1.3
self._cam_yaw = 180
self._cam_pitch = -40
self._h_table = []
self._target_dist_min = 0.05
self._rnd_obj_pose = rnd_obj_pose
# Initialize PyBullet simulator
self._p = p
if self._renders:
self._cid = p.connect(p.SHARED_MEMORY)
if (self._cid < 0):
self._cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(2.5, 90, -60, [0.0, -0.0, -0.0])
else:
self._cid = p.connect(p.DIRECT)
# initialize simulation environment
self.seed()
self.reset()
observationDim = len(self._observation)
observation_high = np.array([largeValObservation] * observationDim)
self.observation_space = spaces.Box(-observation_high,
observation_high,
dtype='float32')
if (self._isDiscrete):
self.action_space = spaces.Discrete(
13) if self._icub.useOrientation else spaces.Discrete(7)
else:
action_dim = self._icub.getActionDimension()
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high,
action_high,
dtype='float32')
self.viewer = None
p.changeDynamics(sphere, -1, linearDamping=0.9)
p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1])
forward = 0
turn = 0
forwardVec = [2, 0, 0]
cameraDistance = 1
cameraYaw = 35
cameraPitch = -35
while (1):
spherePos, orn = p.getBasePositionAndOrientation(sphere)
cameraTargetPosition = spherePos
p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition)
camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5]
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
turn = -0.5
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
turn = 0.5
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
turn = 0
def evaluate(actor_critic, ob_rms, env_name, act_func, seed, k):
num_stack = 1
log_interval = 1
load_dir = './trained_models/ppo/'
#sd = np.random.randint(1000, size=1)
sd = [1]
env = make_env(env_name, int(sd[0]), 0,
'./tmp/test/' + str(seed) + '_' + act_func + '_')
env = DummyVecEnv([env])
#actor_critic, ob_rms = \
# torch.load(os.path.join(load_dir, 'ppo_'+env_name+'_'+act_func+ '_'+str(seed)+".pt"))
if len(env.observation_space.shape) == 1:
env = VecNormalize(env, ret=False)
env.ob_rms = ob_rms
# An ugly hack to remove updates
def _obfilt(self, obs):
if self.ob_rms:
obs = np.clip((obs - self.ob_rms.mean) /
np.sqrt(self.ob_rms.var + self.epsilon),
-self.clipob, self.clipob)
return obs
else:
return obs
env._obfilt = types.MethodType(_obfilt, env)
render_func = env.venv.envs[0].render
else:
render_func = env.envs[0].render
obs_shape = env.observation_space.shape
obs_shape = (obs_shape[0] * num_stack, *obs_shape[1:])
current_obs = torch.zeros(1, *obs_shape)
states = torch.zeros(1, actor_critic.state_size)
masks = torch.zeros(1, 1)
if torch.cuda.is_available():
current_obs = current_obs.cuda()
states = states.cuda()
masks = masks.cuda()
def update_current_obs(obs):
shape_dim0 = env.observation_space.shape[0]
obs = torch.from_numpy(obs).float()
if num_stack > 1:
current_obs[:, :-shape_dim0] = current_obs[:, shape_dim0:]
current_obs[:, -shape_dim0:] = obs
#render_func('human')
obs = env.reset()
update_current_obs(obs)
if env_name.find('Bullet') > -1:
import pybullet as p
torsoId = -1
for i in range(p.getNumBodies()):
if (p.getBodyInfo(i)[0].decode() == "torso"):
torsoId = i
for i in range(5000):
value, action, _, states = actor_critic.act(Variable(current_obs,
volatile=True),
Variable(states,
volatile=True),
Variable(masks,
volatile=True),
deterministic=True)
#if i % 1000 == 0:
# print("STEP: ", i)
states = states.data
cpu_actions = action.data.squeeze(1).cpu().numpy()
# Obser reward and next obs
obs, reward, done, _ = env.step(cpu_actions)
masks.fill_(0.0 if done else 1.0)
if current_obs.dim() == 4:
current_obs *= masks.unsqueeze(2).unsqueeze(2)
else:
current_obs *= masks
update_current_obs(obs)
if env_name.find('Bullet') > -1:
if torsoId > -1:
distance = 5
yaw = 0
humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId)
p.resetDebugVisualizerCamera(distance, yaw, -20, humanPos)
#render_func('human')
#print(dir(env.envs[0]))
process_file(env_name, act_func, sd[0], seed, k)
else: p.connect(p.DIRECT) useZUp = False useYUp = not useZUp if useYUp: p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1) from pdControllerExplicit import PDControllerExplicitMultiDof from pdControllerStable import PDControllerStableMultiDof explicitPD = PDControllerExplicitMultiDof(p) stablePD = PDControllerStableMultiDof(p) p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16]) import pybullet_data p.setTimeOut(10000) useMotionCapture=False useMotionCaptureReset=False#not useMotionCapture useExplicitPD = True p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=30) #p.setPhysicsEngineParameter(solverResidualThreshold=1e-30) #explicit PD control requires small timestep timeStep = 1./600. #timeStep = 1./240.
def __init__(self,
block_random=0.3,
camera_random=0,
simple_observations=False,
continuous=False,
remove_height_hack=False,
urdf_list=None,
render_mode='GUI',
num_objects=5,
dv=0.06,
target=False,
target_filenames=None,
non_target_filenames=None,
num_resets_per_setup=1,
render_width=128,
render_height=128,
downsample_width=64,
downsample_height=64,
test=False,
allow_duplicate_objects=True,
max_num_training_models=900,
max_num_test_models=100):
"""Creates a KukaGraspingEnv.
Args:
block_random: How much randomness to use in positioning blocks.
camera_random: How much randomness to use in positioning camera.
simple_observations: If True, observations are the position and
orientation of end-effector and closest block, rather than images.
continuous: If True, actions are continuous, else discrete.
remove_height_hack: If True and continuous is True, add a dz
component to action space.
urdf_list: List of objects to populate the bin with.
render_mode: GUI, DIRECT, or TCP.
num_objects: The number of random objects to load.
dv: Velocity magnitude of cartesian dx, dy, dz actions per time step.
target: If True, then we receive reward only for grasping one "target"
object.
target_filenames: Objects that we want to grasp.
non_target_filenames: Objects that we dont want to grasp.
num_resets_per_setup: How many env resets before calling setup again.
render_width: Width of camera image to render with.
render_height: Height of camera image to render with.
downsample_width: Width of image observation.
downsample_height: Height of image observation.
test: If True, uses test split of objects.
allow_duplicate_objects: If True, samples URDFs with replacement.
max_num_training_models: The number of distinct models to choose from when
selecting the num_objects placed in the tray for training.
max_num_test_models: The number of distinct models to choose from when
selecting the num_objects placed in the tray for testing.
"""
self._time_step = 1. / 200.
self._max_steps = 15
# Open-source search paths.
self._urdf_root = OSS_DATA_ROOT
self._models_dir = os.path.join(self._urdf_root, 'random_urdfs')
self._action_repeat = 200
self._env_step = 0
self._renders = render_mode in ['GUI', 'TCP']
# Size we render at.
self._width = render_width
self._height = render_height
# Size we downsample to.
self._downsample_width = downsample_width
self._downsample_height = downsample_height
self._target = target
self._num_objects = num_objects
self._dv = dv
self._urdf_list = urdf_list
if target_filenames:
target_filenames = [
self._get_urdf_path(f) for f in target_filenames
]
if non_target_filenames:
non_target_filenames = [
self._get_urdf_path(f) for f in non_target_filenames
]
self._object_filenames = (target_filenames
or []) + (non_target_filenames or [])
self._target_filenames = target_filenames or []
self._block_random = block_random
self._cam_random = camera_random
self._simple_obs = simple_observations
self._continuous = continuous
self._remove_height_hack = remove_height_hack
self._resets = 0
self._num_resets_per_setup = num_resets_per_setup
self._test = test
self._allow_duplicate_objects = allow_duplicate_objects
self._max_num_training_models = max_num_training_models
self._max_num_test_models = max_num_test_models
if render_mode == 'GUI':
self.cid = pybullet.connect(pybullet.GUI)
pybullet.resetDebugVisualizerCamera(1.3, 180, -41,
[0.52, -0.2, -0.33])
elif render_mode == 'DIRECT':
self.cid = pybullet.connect(pybullet.DIRECT)
elif render_mode == 'TCP':
self.cid = pybullet.connect(pybullet.TCP, 'localhost', 6667)
self.setup()
if self._continuous:
self.action_space = spaces.Box(low=-1, high=1, shape=(4, ))
if self._remove_height_hack:
self.action_space = spaces.Box(
low=-1, high=1, shape=(5, )) # dx, dy, dz, da, close
else:
self.action_space = spaces.Discrete(8)
if self._remove_height_hack:
self.action_space = spaces.Discrete(10)
if self._simple_obs:
# (3 pos + 4 quat) x 2
self.observation_space = spaces.Box(low=-100,
high=100,
shape=(14, ))
else:
# image (self._height, self._width, 3) x position of the gripper (3,)
img_space = spaces.Box(low=0,
high=255,
shape=(self._downsample_height,
self._downsample_width, 3))
pos_space = spaces.Box(low=-5, high=5, shape=(3, ))
self.observation_space = spaces.Tuple((img_space, pos_space))
self.viewer = None
def move_and_look_at(self,i,j,k,x,y,z): lookat = [x,y,z] distance = 10 yaw = 10 p.resetDebugVisualizerCamera(distance, yaw, -20, lookat)
targetVelS = 0
width = 512 #Setting parameters for the camera image
height = 512
fov = 60
aspect = width / height
near = 0.02 #Near plane
far = 6
kp = 0.1
kd = 0.9
change = False # Whether Lanes can be changed or not
j = 0 # Counter
prevOrNext = 1 # To determine whether the searcher should go to the next lane or the previous lane while searching
EPSILON = 0.004 # Maximum Possible Error while turning
p.resetDebugVisualizerCamera(10, 0, -89, [10,10,10])
# Loading Model for Detection
detector=CustomObjectDetection()
detector.setModelTypeAsYOLOv3()
detector.setModelPath('detection_model-ex-005--loss-0004.657.h5')
detector.setJsonPath('detection_config.json')
detector.loadModel()
def mapp():
'''
To Initialize the depth map.
'''
img = np.zeros((400,400,3),dtype = int)
for i in range(400):
erp_rb_Id= p.addUserDebugParameter("erp_rb",0,1,defaultERP)
relPosTarget_rb_Id= p.addUserDebugParameter("relPosTarget_rb",-limitVal,limitVal,0)
motA_lb_Id= p.addUserDebugParameter("motA_lb",-limitVal,limitVal,-legpos)
motB_lb_Id= p.addUserDebugParameter("motB_lb",-limitVal,limitVal,-legpos)
motC_lb_Id= p.addUserDebugParameter("motC_lb",-limitVal,limitVal,legposSleft)
erp_lb_Id= p.addUserDebugParameter("erp_lb",0,1,defaultERP)
relPosTarget_lb_Id= p.addUserDebugParameter("relPosTarget_lb",-limitVal,limitVal,0)
camTargetPos=[0.25,0.62,-0.15]
camDist = 2
camYaw = -2
camPitch=-16
p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos)
c_rf = p.createConstraint(vision,knee_front_rightR_joint,vision,motor_front_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,maxForce=maxGearForce)
c_lf = p.createConstraint(vision,knee_front_leftL_joint,vision,motor_front_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,maxForce=maxGearForce)
c_rb = p.createConstraint(vision,knee_back_rightR_joint,vision,motor_back_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,maxForce=maxGearForce)
c_lb = p.createConstraint(vision,knee_back_leftL_joint,vision,motor_back_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
objs = p.loadSDF("botlab/botlab.sdf", globalScaling=2.0)
zero = [0, 0, 0]
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
print("converting y to z axis")
for o in objs:
pos, orn = p.getBasePositionAndOrientation(o)
y2x = p.getQuaternionFromEuler([3.14 / 2., 0, 3.14 / 2])
newpos, neworn = p.multiplyTransforms(zero, y2x, pos, orn)
p.resetBasePositionAndOrientation(o, newpos, neworn)
else:
p.loadURDF("plane.urdf", [0, 0, -3])
p.loadURDF("boston_box.urdf", [-2, 3, -2], useFixedBase=True)
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=148,
cameraPitch=-9,
cameraTargetPosition=[0.36, 5.3, -0.62])
p.loadURDF("boston_box.urdf", [0, 3, -2], useFixedBase=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.getCameraImage(320, 200) #, renderer=p.ER_BULLET_HARDWARE_OPENGL )
t = 0
p.setRealTimeSimulation(1)
while (1):
p.setGravity(0, 0, -10)
time.sleep(0.01)
t += 0.01
keys = p.getKeyboardEvents()
import pybullet as p
import time
import math
p.connect(p.GUI)
#don't create a ground plane, to allow for gaps etc
p.resetSimulation()
#p.createCollisionShape(p.GEOM_PLANE)
#p.createMultiBody(0,0)
#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
#a few different ways to create a mesh:
vertices = [[-0.246350, -0.246483, -0.000624], [-0.151407, -0.176325, 0.172867],
[-0.246350, 0.249205, -0.000624], [-0.151407, 0.129477, 0.172867],
[0.249338, -0.246483, -0.000624], [0.154395, -0.176325, 0.172867],
[0.249338, 0.249205, -0.000624], [0.154395, 0.129477, 0.172867]]
indices = [
0, 3, 2, 3, 6, 2, 7, 4, 6, 5, 0, 4, 6, 0, 2, 3, 5, 7, 0, 1, 3, 3, 7, 6, 7, 5, 4, 5, 1, 0, 6, 4,
0, 3, 1, 5
]
#convex mesh from obj
stoneId = p.createCollisionShape(p.GEOM_MESH, vertices=vertices, indices=indices)
boxHalfLength = 0.5
def update_camera(robot): base_pos = np.array(pybullet.getBasePositionAndOrientation(robot)[0]) [yaw, pitch, dist] = pybullet.getDebugVisualizerCamera()[8:11] pybullet.resetDebugVisualizerCamera(dist, yaw, pitch, base_pos) return
