def _reset(self): if (self.physicsClientId<0): conInfo = p.getConnectionInfo() if (conInfo['isConnected']): self.ownsPhysicsClient = False self.physicsClientId = 0 else: self.ownsPhysicsClient = True self.physicsClientId = p.connect(p.SHARED_MEMORY) if (self.physicsClientId<0): if (self.isRender): self.physicsClientId = p.connect(p.GUI) else: self.physicsClientId = p.connect(p.DIRECT) p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) if self.scene is None: self.scene = self.create_single_player_scene() if not self.scene.multiplayer and self.ownsPhysicsClient: self.scene.episode_restart() self.robot.scene = self.scene self.frame = 0 self.done = 0 self.reward = 0 dump = 0 s = self.robot.reset() self.potential = self.robot.calc_potential() return s
def __init__(self, connection_mode=pybullet.DIRECT, options=""):
"""Create a simulation and connect to it."""
self._client = pybullet.connect(pybullet.SHARED_MEMORY)
if (self._client < 0):
print("options=", options)
self._client = pybullet.connect(connection_mode, options=options)
self._shapes = {}
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=50,
isEnableSelfCollision=True,
renders=True):
print("init")
self._timeStep = 0.01
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._p = p
if self._renders:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
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(9)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.viewer = None
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): # initialize random seed np.random.seed(int(time.time())) cid = p.connect(p.SHARED_MEMORY) # only show graphics if the browser is already running.... self.visualize = (cid >= 0) if cid < 0: cid = p.connect(p.DIRECT) # If no shared memory browser is active, we switch to headless mode (DIRECT is much faster)
def test(args):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
fileName = os.path.join("mjcf", args.mjcf)
print("fileName")
print(fileName)
p.loadMJCF(fileName)
while (1):
p.stepSimulation()
p.getCameraImage(320,240)
time.sleep(0.01)
def main(unused_args):
timeStep = 0.01
c = p.connect(p.SHARED_MEMORY)
if (c<0):
c = p.connect(p.GUI)
params = [0.1903581461951056, 0.0006732219568880068, 0.05018085615283363, 3.219916795483583, 6.2406418167980595, 4.189869754607539]
evaluate_func = 'evaluate_desired_motorAngle_2Amplitude4Phase'
energy_weight = 0.01
finalReturn = evaluate_params(evaluateFunc = evaluate_func, params=params, objectiveParams=[energy_weight], timeStep=timeStep, sleepTime=timeStep)
print(finalReturn)
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 testJacobian(self):
import pybullet as p
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
urdfs = [
"TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf",
"kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf"
]
for urdf in urdfs:
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
robotId = p.loadURDF(urdf, useFixedBase=True)
p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1])
numJoints = p.getNumJoints(robotId)
endEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)])
p.stepSimulation()
# Get the joint and link state directly from Bullet.
mpos, mvel, mtorq = self.getMotorJointStates(robotId)
result = p.getLinkState(robotId,
endEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
# The localPosition is always defined in terms of the link frame coordinates.
zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec,
zero_vec)
assert (allclose(dot(jac_t, mvel), link_vt))
assert (allclose(dot(jac_r, mvel), link_vr))
p.disconnect()
def test_rolling_friction(self):
import pybullet as p
p.connect(p.DIRECT)
p.loadURDF("plane.urdf")
sphere = p.loadURDF("sphere2.urdf", [0, 0, 1])
p.resetBaseVelocity(sphere, linearVelocity=[1, 0, 0])
p.changeDynamics(sphere, -1, linearDamping=0, angularDamping=0)
#p.changeDynamics(sphere,-1,rollingFriction=0)
p.setGravity(0, 0, -10)
for i in range(1000):
p.stepSimulation()
vel = p.getBaseVelocity(sphere)
self.assertLess(vel[0][0], 1e-10)
self.assertLess(vel[0][1], 1e-10)
self.assertLess(vel[0][2], 1e-10)
self.assertLess(vel[1][0], 1e-10)
self.assertLess(vel[1][1], 1e-10)
self.assertLess(vel[1][2], 1e-10)
p.disconnect()
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)
def __init__(self, connection_mode=None):
"""Creates a Bullet client and connects to a simulation.
Args:
connection_mode:
`None` connects to an existing simulation or, if fails, creates a
new headless simulation,
`pybullet.GUI` creates a new simulation with a GUI,
`pybullet.DIRECT` creates a headless simulation,
`pybullet.SHARED_MEMORY` connects to an existing simulation.
"""
self._shapes = {}
if connection_mode is None:
self._client = pybullet.connect(pybullet.SHARED_MEMORY)
if self._client >= 0:
return
else:
connection_mode = pybullet.DIRECT
self._client = pybullet.connect(connection_mode)
def __init__(self):
# start the bullet physics server
p.connect(p.GUI)
# p.connect(p.DIRECT)
observation_high = np.array([
np.finfo(np.float32).max,
np.finfo(np.float32).max,
np.finfo(np.float32).max,
np.finfo(np.float32).max])
action_high = np.array([0.1])
self.action_space = spaces.Box(-action_high, action_high)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.theta_threshold_radians = 1
self.x_threshold = 2.4
self._seed()
# self.reset()
self.viewer = None
self._configure()
def main(unused_args):
timeStep = 0.01
c = p.connect(p.SHARED_MEMORY)
if (c<0):
c = p.connect(p.GUI)
p.resetSimulation()
p.setTimeStep(timeStep)
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
minitaur = Minitaur()
amplitude = 0.24795664427
speed = 0.2860877729434
for i in range(1000):
a1 = math.sin(i*speed)*amplitude+1.57
a2 = math.sin(i*speed+3.14)*amplitude+1.57
joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57]
minitaur.applyAction(joint_values)
p.stepSimulation()
# print(minitaur.getBasePosition())
time.sleep(timeStep)
final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition()))
print(final_distance)
def __init__(self, renders=True):
# start the bullet physics server
self._renders = renders
if (renders):
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
self.theta_threshold_radians = 12 * 2 * math.pi / 360
self.x_threshold = 0.4 #2.4
high = np.array([
self.x_threshold * 2,
np.finfo(np.float32).max,
self.theta_threshold_radians * 2,
np.finfo(np.float32).max])
self.force_mag = 10
self.action_space = spaces.Discrete(2)
self.observation_space = spaces.Box(-high, high, dtype=np.float32)
self._seed()
# self.reset()
self.viewer = None
self._configure()
def __init__(self, renders=True):
# start the bullet physics server
self._renders = renders
if (renders):
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
observation_high = np.array([
np.finfo(np.float32).max,
np.finfo(np.float32).max,
np.finfo(np.float32).max,
np.finfo(np.float32).max])
action_high = np.array([0.1])
self.action_space = spaces.Discrete(9)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.theta_threshold_radians = 1
self.x_threshold = 2.4
self._seed()
# self.reset()
self.viewer = None
self._configure()
def __enter__(self):
print("connecting")
optionstring='--width={} --height={}'.format(pixelWidth,pixelHeight)
optionstring += ' --window_backend=2 --render_device=0'
print(self.connection_mode, optionstring,*self.argv)
cid = pybullet.connect(self.connection_mode, options=optionstring,*self.argv)
if cid < 0:
raise ValueError
print("connected")
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW,0)
pybullet.resetSimulation()
pybullet.loadURDF("plane.urdf",[0,0,-1])
pybullet.loadURDF("r2d2.urdf")
pybullet.loadURDF("duck_vhacd.urdf")
pybullet.setGravity(0,0,-10)
import pybullet as p
import time
import math
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.setPhysicsEngineParameter(contactBreakingThreshold=0.04)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
[1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
[1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
[1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
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
import pybullet as p
import time
from pybullet_utils import urdfEditor
##########################################
org2 = p.connect(p.DIRECT)
org = p.connect(p.SHARED_MEMORY)
if (org<0):
org = p.connect(p.DIRECT)
gui = p.connect(p.GUI)
p.resetSimulation(physicsClientId=org)
#door.urdf, hinge.urdf, duck_vhacd.urdf, r2d2.urdf, quadruped/quadruped.urdf
mb = p.loadURDF("r2d2.urdf", flags=p.URDF_USE_IMPLICIT_CYLINDER, physicsClientId=org)
for i in range(p.getNumJoints(mb,physicsClientId=org)):
p.setJointMotorControl2(mb,i,p.VELOCITY_CONTROL,force=0,physicsClientId=org)
#print("numJoints:",p.getNumJoints(mb,physicsClientId=org))
#print("base name:",p.getBodyInfo(mb,physicsClientId=org))
#for i in range(p.getNumJoints(mb,physicsClientId=org)):
import pybullet as p
import numpy as np
import time
from AgentReacher import Reacher
np.set_printoptions(suppress=True)
np.set_printoptions(formatter={'float': '{: 0.2f}'.format})
timeStep = 0.016
c = p.connect(p.GUI)
p.resetSimulation()
p.setTimeStep(timeStep)
p.setGravity(0, 0, -9.8)
p.setRealTimeSimulation(1)
reacher = Reacher()
while True:
for i in range(50):
action = np.random.rand(2) * 2 - 1
obs, reward, done, info = reacher.step(action)
print(obs)
time.sleep(timeStep)
reacher.reset()
def resetRobot(RoboBoi):
p.resetBasePositionAndOrientation(RoboBoi, originalPos,
originalOrientation)
for joint in range(p.getNumJoints(RoboBoi)):
p.resetJointState(RoboBoi, joint, 0)
p.setJointMotorControl2(RoboBoi, joint, p.VELOCITY_CONTROL, 0)
#p.setJointMotorControl2(RoboBoi,joint,p.TORQUE_CONTROL,0)
p.setJointMotorControl2(RoboBoi, 9, p.POSITION_CONTROL, -0.4)
p.setJointMotorControl2(RoboBoi, 6, p.POSITION_CONTROL, -0.4)
return
## Loading
physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
RobotStartPosition = [0, 0, 0.75]
RobotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
RoboBoi = p.loadURDF("Robot.urdf", RobotStartPosition, RobotStartOrientation)
# Create Parent-Child Map
parent_child_map = numpy.array([[0, 1], [1, 2], [0, 3], [3, 4], [4, 5], \
[3, 6], [6, 7], [7, 8], [0, 9], [9, 10], [10, 11]])
## Print all the links
for i in range(p.getNumJoints(RoboBoi)):
link = p.getJointInfo(RoboBoi, i)
print(link)
def __init__(self, animate=False, max_steps=1000):
self.animate = animate
self.max_steps = max_steps
# Initialize simulation
if (animate):
self.client_ID = p.connect(p.GUI)
else:
self.client_ID = p.connect(p.DIRECT)
assert self.client_ID != -1, "Physics client failed to connect"
# Set simulation world params
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 120.)
# Input and output dimensions defined in the environment
self.obs_dim = 12
self.act_dim = 2
# sum rewards
self.lastrew = 0
# parametrs for the track
self.ctr = 0
self.iteration = 100
self.first = True
self.X = []
self.Y = []
self.index = 0
self.amount = 0
self.reset_index = 0
env_width = 100
env_height = 100
# generating track
heightfieldData = [0] * (env_height * env_width)
# if we would like to generate new track every time
'''
datasetx, datasety, x1,y1,cx,cy = getdataset()
datasetx = np.int64(datasetx)
datasety = np.int64(datasety)
x1 = np.int64(x1)
y1 = np.int64(y1)'''
# for training we used pregenerated tracks
if self.animate:
self.tracking = 5
else:
self.tracking = 1
exx = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'outx0.npy')
exy = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'outy0.npy')
inx = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'inx0.npy')
iny = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'iny0.npy')
checkx = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'checkpointsx0.npy')
checky = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'checkpointsy0.npy')
datasetx = np.load(exx)
datasety = np.load(exy)
x1 = np.load(inx)
y1 = np.load(iny)
cx = np.load(checkx)
cy = np.load(checky)
first = True
for i in range(len(datasetx)):
x = (datasetx[i])
y = (datasety[i])
xnew = x1[i]
ynew = y1[i]
if not first and xold == x and yold == y:
continue
else:
heightfieldData.pop(env_height * x + y)
heightfieldData.insert(env_height * x + y, 1)
heightfieldData.pop(env_height * xnew + ynew)
heightfieldData.insert(env_height * xnew + ynew, 1)
xold = x
yold = y
first = False
self.X = cy
self.Y = cx
self.amount = len(cx)
terrainShape = p.createCollisionShape(
shapeType=p.GEOM_HEIGHTFIELD,
meshScale=[1, 1, 1],
heightfieldTextureScaling=(env_width - 1) / 2,
heightfieldData=heightfieldData,
numHeightfieldRows=env_height,
numHeightfieldColumns=env_width,
physicsClientId=self.client_ID)
mass = 0
terrain = p.createMultiBody(mass, terrainShape)
p.resetBasePositionAndOrientation(terrain, [49.5, 49.5, 0],
[0, 0, 0, 1])
# generating car in the right angle
if (self.X[self.reset_index + 5] - self.X[self.reset_index]) < 0 and (
(self.Y[self.reset_index + 5] - self.Y[self.reset_index]) < 2 and
(self.Y[self.reset_index + 5] - self.Y[self.reset_index]) > -2):
angle = 100
elif (self.X[self.reset_index + 5] - self.X[self.reset_index]) > 0 and (
(self.Y[self.reset_index + 5] - self.Y[self.reset_index]) < 2 and
(self.Y[self.reset_index + 5] - self.Y[self.reset_index]) > -2):
angle = 0
elif (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) < 0 and (
(self.X[self.reset_index + 5] - self.X[self.reset_index]) < 2 and
(self.X[self.reset_index + 5] - self.X[self.reset_index]) > -2):
angle = -1
elif (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) > 0 and (
(self.X[self.reset_index + 5] - self.X[self.reset_index]) < 2 and
(self.X[self.reset_index + 5] - self.X[self.reset_index]) > -2):
angle = 1
elif (self.X[self.reset_index + 5] -
self.X[self.reset_index]) > 0 and (self.Y[self.reset_index + 5] -
self.Y[self.reset_index]) > 0:
angle = 0.5
elif (self.X[self.reset_index + 5] -
self.X[self.reset_index]) > 0 and (self.Y[self.reset_index + 5] -
self.Y[self.reset_index]) < 0:
angle = -0.5
elif (self.X[self.reset_index + 5] -
self.X[self.reset_index]) < 0 and (self.Y[self.reset_index + 5] -
self.Y[self.reset_index]) < 0:
angle = -2
elif (self.X[self.reset_index + 5] -
self.X[self.reset_index]) < 0 and (self.Y[self.reset_index + 5] -
self.Y[self.reset_index]) > 0:
angle = 2
else:
angle = 1
self.car = p.loadURDF("f10_racecar/racecar_differential.urdf",
[self.X[0], self.Y[0], .3], [0, 0, angle, 1])
p.resetDebugVisualizerCamera(
cameraDistance=10,
cameraYaw=0,
cameraPitch=270,
cameraTargetPosition=[self.X[0], self.Y[0], 0])
# Input and output dimensions defined in the environment
for wheel in range(p.getNumJoints(self.car)):
p.setJointMotorControl2(self.car,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
p.changeDynamics(self.car, wheel, mass=1, lateralFriction=1.0)
self.wheels = [8, 15]
# spojuje klouby přes ramena dohromady a tím vytváří celek auta
self.c = p.createConstraint(self.car,
9,
self.car,
11,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c, gearRatio=1, maxForce=10000)
self.c = p.createConstraint(self.car,
10,
self.car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c, gearRatio=-1, maxForce=10000)
self.c = p.createConstraint(self.car,
9,
self.car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c, gearRatio=-1, maxForce=10000)
self.c = p.createConstraint(self.car,
16,
self.car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c, gearRatio=1, maxForce=10000)
self.c = p.createConstraint(self.car,
16,
self.car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c, gearRatio=-1, maxForce=10000)
self.c = p.createConstraint(self.car,
17,
self.car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c, gearRatio=-1, maxForce=10000)
self.c = p.createConstraint(self.car,
1,
self.car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c,
gearRatio=-1,
gearAuxLink=15,
maxForce=10000)
self.c = p.createConstraint(self.car,
3,
self.car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c,
gearRatio=-1,
gearAuxLink=15,
maxForce=10000)
self.steering = [0, 2]
# Limits of our joints. When using the * (multiply) operation on a list, it repeats the list that many times
self.joints_rads_low = -1.57
self.joints_rads_high = 4.71
self.joints_rads_diff = self.joints_rads_high - self.joints_rads_low
# self.Velocity = 1
self.stepCtr = 0
self.hokuyo_joint = 4
# Lidar rays initialisation
self.replaceLines = True
self.numRays = 10
self.rayFrom = []
self.rayTo = []
self.rayIds = []
self.rayHitColor = [1, 0, 0]
self.rayMissColor = [0, 1, 0]
self.rayLen = 7
self.rayStartLen = 0.25
for i in range(self.numRays):
self.rayFrom.append([
self.rayStartLen *
math.sin(-0.5 * 0.25 * 2. * math.pi +
0.75 * 2. * math.pi * float(i) / self.numRays),
self.rayStartLen *
math.cos(-0.5 * 0.25 * 2. * math.pi +
0.75 * 2. * math.pi * float(i) / self.numRays), 0
])
self.rayTo.append([
self.rayLen *
math.sin(-0.5 * 0.25 * 2. * math.pi +
0.75 * 2. * math.pi * float(i) / self.numRays),
self.rayLen *
math.cos(-0.5 * 0.25 * 2. * math.pi +
0.75 * 2. * math.pi * float(i) / self.numRays), 0
])
if (self.replaceLines):
self.rayIds.append(
p.addUserDebugLine(self.rayFrom[i],
self.rayTo[i],
self.rayMissColor,
parentObjectUniqueId=self.car,
parentLinkIndex=self.hokuyo_joint))
else:
self.rayIds.append(-1)
def __init__(self, connection="GUI", prev_epoch=0, seed=None):
np.random.seed(seed)
if connection == "GUI":
self.pybullet = p.connect(p.GUI)
else:
self.pybullet = p.connect(p.DIRECT)
p.setTimeStep(1.0 / SIMULATIONFREQUENCY)
p.setGravity(0, 0, -9.81)
self.ground = p.loadURDF(
"/home/shawn/Documents/ANYmal/assets/plane.urdf", [0, 0, 0])
self.anymal = p.loadURDF(
"/home/shawn/Documents/DRL/JueyingProURDF/urdf/jueying.urdf",
[0.0, 0.0, 1.0])
self.action_space = spaces.Box(low=-2 * np.pi * np.ones(12),
high=2 * np.pi * np.ones(12))
self.actionTime = 3.0
self.t = 0.0
self.epoch = prev_epoch # The number of DRL epochs that have been done
self.lastTime = 0.0 # Used to add noise
self.torqueNoise = 0.0
self.positionNoise = 0.0
maxPosition = np.pi * np.ones(12)
maxVelocity = 2 * np.ones(12)
maxPositionHistory = np.pi * np.ones(24)
maxVelocityHistory = 2 * np.ones(24)
maxBasePositionAndOrientation = np.ones(7)
maxBasePosition = np.ones(3)
maxBaseOrientationVector = np.ones(3)
maxBaseVelocity = 1 * np.ones(3)
maxBaseAngularVelocity = 2 * np.pi * np.ones(3)
maxAcceleration = np.inf * np.ones(12)
observationUpperBound = np.concatenate([
maxPosition, maxVelocity, maxBasePositionAndOrientation,
maxBaseOrientationVector, maxBaseVelocity, maxBaseAngularVelocity,
maxAcceleration
])
stateUpperBound = np.concatenate([
maxBaseOrientationVector, maxBaseVelocity, maxBaseAngularVelocity,
maxPosition, maxVelocity, maxPositionHistory, maxVelocityHistory,
maxPosition
])
# self.observation_space = spaces.Box(
# low=-observationUpperBound,
# high=observationUpperBound
# )
self.observation_space = spaces.Box(low=-stateUpperBound,
high=stateUpperBound)
self.history_buffer = dict(
) # used to storage the joint state history and the last action
self.history_buffer['last_action'] = np.nan * np.ones(12)
self.history_buffer['joint_state'] = {
'time': [],
'position': [],
'velocity': [],
'position_error': []
}
self.buffer_time_length = 0.03
p.setJointMotorControlArray(self.anymal,
[joint.value for joint in Joints],
p.POSITION_CONTROL,
targetPositions=np.zeros(12),
forces=np.zeros(12))
j,
p.POSITION_CONTROL,
targetPosition,
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[0, 0, 0])
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
p.setJointMotorControl2(body_id,
j,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
physicsClient = p.connect(p.GUI)
body_id = p.loadURDF("../data/man_on_qolo/man_x_partitioned_on_qolo.urdf",
flags=p.URDF_MAINTAIN_LINK_ORDER)
prepare_man_on_qolo(body_id)
disable_body_motors(body_id)
colBoxId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[5, 5, 5])
box_ID = p.createMultiBody(0, colBoxId, -1, [0, 0, -5], [0, 0, 0, 1])
#time.sleep(1)
for i in range(5000):
p.stepSimulation()
time.sleep(1 / 240.0)
def start():
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
# p.setDefaultContactERP(0)
# urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago_toes.urdf", [0, 0, .5],
[0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
# enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
# 2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=",
enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5,
enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
# print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
# print (currentline)
# print("-----")
frame = currentline[0]
t = currentline[1]
# print("frame[",frame,"]")
joints = currentline[2:14]
# print("joints=",joints)
for j in range(12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos +
jointOffsets[j],
force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
# print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg)
# print("num points=",len(pts))
# for pt in pts:
# print(pt[9])
time.sleep(1. / 500.)
index = 0
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
js = p.getJointState(quadruped, j)
# print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
(js[0] - jointOffsets[index]) /
jointDirections[index]))
index = index + 1
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,
jointIds[i],
p.POSITION_CONTROL,
jointDirections[i] * targetPos +
jointOffsets[i],
force=maxForce)
def start_client(self):
self.client = p.connect(p.GUI) if (self._gui is True) else p.connect(p.DIRECT)
p.setGravity(0, 0, -1 * self.G)
p.setTimeStep(self.physics_sample_time)
p.setRealTimeSimulation(self._real_time)
#get robot models
#kdl
ok, snake_tree = kdlurdf.treeFromFile(path_to_urdf)
snake_chain = snake_tree.getChain(root, tip)
#rbdl
snake_rbdl = rbdl.loadModel(path_to_urdf)
#u2c
snake = u2c.URDFparser()
snake.from_file(path_to_urdf)
#pybullet
sim = pb.connect(pb.DIRECT)
snake_pb = pb.loadURDF(path_to_urdf,
useFixedBase=True,
flags=pb.URDF_USE_INERTIA_FROM_FILE)
#joint info
jointlist, names, q_max, q_min = snake.get_joint_info(root, tip)
n_joints = snake.get_n_joints(root, tip)
#declarations
#kdl
q_kdl = kdl.JntArray(n_joints)
qdot_kdl = kdl.JntArray(n_joints)
gravity_kdl = kdl.Vector()
gravity_kdl[2] = -9.81
contours = cv2.findContours(mask, cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)[0]
for c in contours:
if(cv2.contourArea(c)<500):
(x,y),r = cv2.minEnclosingCircle(c)
r = int(r)
if r!=0:
center = (int(x),int(y))
cv2.circle(image,center,r,(0,255,0),2)
cv2.imshow("Image", image)
cv2.waitKey(0)
cv2.destroyAllWindows()
return center
# setting physics client
PhysicsClent=p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# setting gravity
p.setGravity(0,0,-10)
# setting plane
plane=p.loadURDF("plane.urdf")
# setting table
table_pos=[0,0,0]
table_ori=p.getQuaternionFromEuler([0,0,0])
table=p.loadURDF("table(1).urdf",table_pos,table_ori)
# setting puck
puck_pos=[-0.6,0.0,0.3]
import numpy as np
import pybullet as p
import pybullet_data
import sys, os, time
script_path = os.path.dirname(os.path.realpath(__file__))
os.sys.path.append(os.path.realpath(script_path + '/../'))
from src.buff_digit import Buff_digit
if __name__ == '__main__':
client = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setRealTimeSimulation(1)
p.setGravity(0, 0, -9.81)
p.setPhysicsEngineParameter(enableConeFriction=0)
p.setAdditionalSearchPath('../models')
floor = p.loadURDF('floor/floor.urdf', useFixedBase=True)
robot = Buff_digit(client)
time.sleep(5)
#SE2 poses (x, y, theta)
pose1 = [0.2, -0.25, 0]
pose2 = [-1.0, -1.0, -1.57]
pose3 = [-4.0, 0.5, -3.1415]
pose4 = [-1.0, 1.0, 1.57]
base_limits = ((-2.5, -2.5), (2.5, 2.5))
for i in range(10):
robot.plan_and_drive_to_pose(pose1, base_limits, obstacles=[])
time.sleep(1)
robot.plan_and_drive_to_pose(pose2, base_limits, obstacles=[])
def initPyBullet(self):
p.connect(p.DIRECT) # GUI or DIRECT
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
def __init__(self, config, use_visualizer=False, name=None):
"""Initialize.
Args:
config (dict): YAML config dict containing specifications.
use_visualizer (bool): Whether or not to use visualizer.
Note:: Pybullet only allows for one simulation to be
connected to GUI. It is up to the user to manage this.
name (str): Name of the world. (for multiple worlds)
Returns:
None
Example:
BulletWorld('cfg/my_config.yaml', True, 'MyBulletWorld')
Notes:
The world_factory is the recommended way for creating an
appropriate instance of world according to the config parameters.
Upon initialization the world automatically creates instances of
BulletRobotInterface, BulletSensorInterface and
BulletObjectInterface according to the config dictionary.
"""
# Get configuration parameters
self.config = config
# Connect to appropriate pybullet channel based on use_visualizer flag
self.use_visualizer = use_visualizer
if self.use_visualizer:
print(
"==================USING_VISUALIZER!!!!!=====================")
self._physics_id = pybullet.connect(pybullet.GUI)
else:
print("==============NOT USING VISUALIZER!!!!=================")
self._physics_id = pybullet.connect(pybullet.DIRECT)
logging.info("New PhysicsID: " + str(self._physics_id))
self._time_step = self.config['sim_params']['time_step']
self.set_pb_physics()
# check if config has camera.
self.has_camera = False
if 'sensor' in self.config:
if 'camera' in self.config['sensor']:
self.has_camera = True
self.has_object = False
# check if config has objects.
if 'object' in self.config:
self.has_object = True
# Create an arena to load robot and objects
self.arena = BulletArena(self.config,
self._physics_id,
has_camera=self.has_camera)
self.robot_interface = BulletRobotInterface.create(
config=self.config,
physics_id=self._physics_id,
arm_id=self.arena.arm_id,
controlType=self.config['world']['controlType'])
self.control_freq = self.config['control_freq']
if self.has_camera:
self.camera_interface = BulletCameraInterface(
physics_id=self._physics_id,
image_height=self.config['sensor']['camera']['image']
['height'],
image_width=self.config['sensor']['camera']['image']['width'],
near=self.config['sensor']['camera']['intrinsics']
['near_plane'],
far=self.config['sensor']['camera']['intrinsics']['far_plane'],
fov=self.config['sensor']['camera']['intrinsics']['fov'],
cameraEyePosition=self.config['sensor']['camera']['extrinsics']
['eye_position'],
cameraTargetPosition=self.config['sensor']['camera']
['extrinsics']['target_position'],
cameraUpVector=self.config['sensor']['camera']['extrinsics']
['up_vector'])
if self.has_object:
self._load_object_interfaces()
self.name = name
# self.ctrl_steps_per_action = int((self.config['control_freq'] / float(self.config['policy_freq'] * self.config['sim_params']['time_step'])))
self.ctrl_steps_per_action = int(
(self.config['control_freq'] / float(self.config['policy_freq'])))
self.is_sim = True
self.step_counter = 0
self.ee_list = []
def main():
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter, )
parser.add_argument("model", help="model file in a log dir")
parser.add_argument("--gpu", type=int, default=0, help="gpu id")
parser.add_argument("--save", action="store_true", help="save")
args = parser.parse_args()
args_file = path.Path(args.model).parent / "args"
with open(args_file) as f:
args_data = json.load(f)
pprint.pprint(args_data)
if args.gpu >= 0:
chainer.cuda.get_device_from_id(args.gpu).use()
model = contrib.models.Model(
n_fg_class=len(args_data["class_names"][1:]),
pretrained_resnet18=args_data["pretrained_resnet18"],
centerize_pcd=args_data["centerize_pcd"],
# with_occupancy=args_data['with_occupancy'],
# loss=args_data['loss'],
# loss_scale=args_data['loss_scale'],
)
if args.gpu >= 0:
model.to_gpu()
print(f"==> Loading trained model: {args.model}")
chainer.serializers.load_npz(args.model, model)
print("==> Done model loading")
split = "val"
dataset = morefusion.datasets.YCBVideoRGBDPoseEstimationDataset(
split=split)
dataset_reindexed = morefusion.datasets.YCBVideoRGBDPoseEstimationDatasetReIndexed( # NOQA
split=split,
class_ids=args_data["class_ids"],
)
# transform = Transform(
# train=False,
# with_occupancy=args_data['with_occupancy'],
# )
pprint.pprint(args.__dict__)
# -------------------------------------------------------------------------
depth2rgb = imgviz.Depth2RGB()
for index in range(len(dataset)):
frame = dataset.get_frame(index)
image_id = dataset._ids[index]
indices = dataset_reindexed.get_indices_from_image_id(image_id)
examples = dataset_reindexed[indices]
examples = [transform(example) for example in examples]
if not examples:
continue
inputs = chainer.dataset.concat_examples(examples, device=args.gpu)
with chainer.no_backprop_mode() and chainer.using_config(
"train", False):
quaternion_pred, translation_pred, confidence_pred = model.predict(
class_id=inputs["class_id"],
rgb=inputs["rgb"],
pcd=inputs["pcd"],
# pitch=inputs.get('pitch'),
# origin=inputs.get('origin'),
# grid_nontarget_empty=inputs.get('grid_nontarget_empty'),
)
indices = model.xp.argmax(confidence_pred.array, axis=1)
quaternion_pred = quaternion_pred[
model.xp.arange(quaternion_pred.shape[0]), indices]
translation_pred = translation_pred[
model.xp.arange(translation_pred.shape[0]), indices]
reporter = chainer.Reporter()
reporter.add_observer("main", model)
observation = {}
with reporter.scope(observation):
model.evaluate(
class_id=inputs["class_id"],
quaternion_true=inputs["quaternion_true"],
translation_true=inputs["translation_true"],
quaternion_pred=quaternion_pred,
translation_pred=translation_pred,
)
# TODO(wkentaro)
observation_new = {}
for k, v in observation.items():
if re.match(r"main/add_or_add_s/[0-9]+/.+", k):
k_new = "/".join(k.split("/")[:-1])
observation_new[k_new] = v
observation = observation_new
print(f"[{index:08d}] {observation}")
# ---------------------------------------------------------------------
K = frame["intrinsic_matrix"]
height, width = frame["rgb"].shape[:2]
fovy = trimesh.scene.Camera(resolution=(width, height),
focal=(K[0, 0], K[1, 1])).fov[1]
batch_size = len(inputs["class_id"])
class_ids = cuda.to_cpu(inputs["class_id"])
quaternion_pred = cuda.to_cpu(quaternion_pred.array)
translation_pred = cuda.to_cpu(translation_pred.array)
quaternion_true = cuda.to_cpu(inputs["quaternion_true"])
translation_true = cuda.to_cpu(inputs["translation_true"])
Ts_pred = []
Ts_true = []
for i in range(batch_size):
# T_cad2cam
T_pred = tf.quaternion_matrix(quaternion_pred[i])
T_pred[:3, 3] = translation_pred[i]
T_true = tf.quaternion_matrix(quaternion_true[i])
T_true[:3, 3] = translation_true[i]
Ts_pred.append(T_pred)
Ts_true.append(T_true)
Ts = dict(true=Ts_true, pred=Ts_pred)
vizs = []
depth_viz = depth2rgb(frame["depth"])
for which in ["true", "pred"]:
pybullet.connect(pybullet.DIRECT)
for i, T in enumerate(Ts[which]):
cad_file = morefusion.datasets.YCBVideoModels().get_cad_file(
class_id=class_ids[i])
morefusion.extra.pybullet.add_model(
cad_file,
position=tf.translation_from_matrix(T),
orientation=tf.quaternion_from_matrix(T)[[1, 2, 3, 0]],
)
(
rgb_rend,
depth_rend,
segm_rend,
) = morefusion.extra.pybullet.render_camera(
np.eye(4), fovy, height, width)
pybullet.disconnect()
segm_rend = imgviz.label2rgb(segm_rend + 1,
img=frame["rgb"],
alpha=0.7)
depth_rend = depth2rgb(depth_rend)
rgb_input = imgviz.tile(cuda.to_cpu(inputs["rgb"]),
border=(255, 255, 255))
viz = imgviz.tile(
[
frame["rgb"],
depth_viz,
rgb_input,
segm_rend,
rgb_rend,
depth_rend,
],
(1, 6),
border=(255, 255, 255),
)
viz = imgviz.resize(viz, width=1800)
if which == "pred":
text = []
for class_id in np.unique(class_ids):
add = observation[f"main/add_or_add_s/{class_id:04d}"]
text.append(f"[{which}] [{class_id:04d}]: "
f"add/add_s={add * 100:.1f}cm")
text = "\n".join(text)
else:
text = f"[{which}]"
viz = imgviz.draw.text_in_rectangle(
viz,
loc="lt",
text=text,
size=20,
background=(0, 255, 0),
color=(0, 0, 0),
)
if which == "true":
viz = imgviz.draw.text_in_rectangle(
viz,
loc="rt",
text="singleview_pcd",
size=20,
background=(255, 0, 0),
color=(0, 0, 0),
)
vizs.append(viz)
viz = imgviz.tile(vizs, (2, 1), border=(255, 255, 255))
if args.save:
out_file = path.Path(args.model).parent / f"video/{index:08d}.jpg"
out_file.parent.makedirs_p()
imgviz.io.imsave(out_file, viz)
yield viz
def euler_to_quaternion(r):
(roll, pitch, yaw) = (r[0], r[1], r[2])
qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(
roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(
roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2)
qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(
roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2)
qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(
roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
return [qw, qx, qy, qz]
p.connect(p.DIRECT)
file_path = cwd_path + "/gym_raisim_towr/envs/dll_rsc/rsc/anymal/anymal.urdf"
p.setGravity(0, 0, -10)
geo_anymal = p.loadURDF(file_path)
'''
* ik function with an error of +- 0.14 radians
* since pybullet has a recursice IK solver
we r running stepSimulation 20 times to get
the angles in the final target ee location.
(values 20 was consistent to reach the
target location any where inside the kinematic box of towr)
'''
def pybullet_ik(base_pos, base_quat, ee1, ee2, ee3, ee4):
base_pos = list(base_pos)
# print (', '.join(row))
# cnt += 1
# if first:
# first = False
# continue
# print(row, cnt)
# trajectory_point = {"timestep": row[0], "x": float(row[5]), "y": float(row[6]), "z": float(row[7])}
# if row[4] == "hand":
# print ("found hand")
# trajectory.append(trajectory_point)
# return trajectory
show_gt = args.gt # using this parameter, the robot shows the behaviour with ground-truth input data this can be seen as current best case scenario
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.GUI)
video_filename = os.path.basename(args.csvfile)[:-4]+".mp4"
# print("Log video to: ")
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
# p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, video_filename)
p.loadURDF(config["environment"]["model"],[0,0,0], globalScaling=0.01)
jd = config["jd"]
ll = config["ll"]
ul = config["ul"]
jr = config["jr"]
#!/usr/bin/env python
import time
import math
import pybullet as p
import pybullet_data
CLIENT = p.connect(p.GUI, options='--width=1024 --height=786')
def setup_world():
"""
function to init the world
"""
p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.setRealTimeSimulation(0, CLIENT)
def main():
"""
main function
"""
setup_world()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
# urdf_fn = '/Users/krishneelchaudhary/Downloads/bullet3/data/kuka_lwr/kuka.urdf'
urdf_fn = '/Users/krishneelchaudhary/Downloads/bullet3/examples/pybullet/gym/pybullet_data/kuka_iiwa/model_for_sdf.urdf'
def initPhysicsClient():
physicsClient = p.connect(p.GUI)
p.setGravity(0, 0, -9.81)
p.setPhysicsEngineParameter(fixedTimeStep=dt, numSubSteps=1)
return physicsClient
"Expected torque vector of "
"length {}, got {}".format(num_joints, len(torque)))
def multiplyJacobian(robot, jacobian, vector):
result = [0.0, 0.0, 0.0]
i = 0
for c in range(len(vector)):
if p.getJointInfo(robot, c)[3] > -1:
for r in range(3):
result[r] += jacobian[r][i] * vector[c]
i += 1
return result
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
time_step = 0.001
gravity_constant = -9.81
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
p.loadURDF("plane.urdf", [0, 0, -0.3])
kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True)
#kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
def __init__(self):
# initial ros node
rospy.init_node('collision_check', anonymous = True)
# get joint state data
self.jointStateAddress = "/j2n6s300_driver/out/joint_state"
self.joint_pos = np.array([0.0] * 6)
self.kinova_joint_pos = np.array([0.0] * 6)
rospy.Subscriber(self.jointStateAddress, JointState, self.jointFeedback)
# get fridge location relative to the kinova base link
# try:
self._listener = tf.TransformListener()
self._tfBuffer = tf2_ros.Buffer()
self._listener2 = tf2_ros.TransformListener(self._tfBuffer)
startTime = time.time()
while True:
if time.time() - startTime > 10:
fridge_pos = [0.1568307262, 0.626700624618, 0.308028843394]
print "Can not find tfBuffer. Use pre-defined value"
break
try:
T_base2tag = self._tfBuffer.lookup_transform('j2n6s300_link_base', 'marker_frame', rospy.Time())
fridge_pos = np.array([ T_base2tag.transform.translation.x,
T_base2tag.transform.translation.y,
T_base2tag.transform.translation.z
])
print "Get correct tf from base to marker"
break
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as err:
# print err
pass
# except:
# tag_pos = [0.1568307262, 0.626700624618, 0.308028843394]
rospy.sleep(1)
self.physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-10)
kinovaStartPos = [0,0,0.675]
kinovaStartOri = p.getQuaternionFromEuler([0,0,0])
# two objects
self.planeId = p.loadURDF("plane.urdf") # environment the file is in bullet default folder
self.kinova = p.loadURDF("data/model2.urdf", kinovaStartPos, kinovaStartOri, useFixedBase=True)
self.box = p.loadURDF("data/table.urdf", [0,1.4,0.38], kinovaStartOri, useFixedBase=True)
# self.box = p.loadURDF("data/cube.urdf", [0,1.3,0],cubeStartOrientation)
# self.box = p.loadSDF("data/ketchen.model")
# self.box = p.loadURDF("data/kitchen_object.urdf")
# self.fridge = p.loadURDF("kitchen/fridge.urdf", [0,1.7,1],cubeStartOrientation)
# self.joint_pos = np.array([0., pi/2, pi/2, pi/2, pi/2, 0])
self.fridge = p.loadURDF("kitchen/fridge.urdf", [fridge_pos[0],fridge_pos[1]+0.075,fridge_pos[2]-0.18+0.675],kinovaStartOri, useFixedBase=True )
self.kinovaEndEffector = 7
self.jacobian = None
self.closestPts = None
p.setRealTimeSimulation(1)
import pybullet as p
import time
import pkgutil
egl = pkgutil.get_loader('eglRenderer')
p.connect(p.DIRECT)
plugin = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
print("plugin=",plugin)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setGravity(0,0,-10)
p.loadURDF("plane.urdf",[0,0,-1])
p.loadURDF("r2d2.urdf")
pixelWidth = 320
pixelHeight = 220
camTargetPos = [0,0,0]
camDistance = 4
pitch = -10.0
roll=0
upAxisIndex = 2
while (p.isConnected()):
for yaw in range (0,360,10) :
start = time.time()
p.stepSimulation()
#script to control a simulated robot hand using a VR glove
#see https://twitter.com/erwincoumans/status/821953216271106048
#and https://www.youtube.com/watch?v=I6s37aBXbV8
#vr glove was custom build using Spectra Symbolflex sensors (4.5")
#inside a Under Armour Batting Glove, using DFRobot Bluno BLE/Beetle
#with BLE Link to receive serial (for wireless bluetooth serial)
import serial
import time
import pybullet as p
import pybullet_data
#first try to connect to shared memory (VR), if it fails use local GUI
c = p.connect(p.SHARED_MEMORY)
if (c < 0):
c = p.connect(p.GUI)
#p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
print(c)
if (c < 0):
p.connect(p.GUI)
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl2.xml")
hand = objects[0]
ho = p.getQuaternionFromEuler([0, 3.14, 0])
hand_cid = p.createConstraint(hand, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0],
[0.1, 0, 0], [0.500000, 0.300006, 0.700000], ho)
import pybullet as p
from time import sleep
import matplotlib.pyplot as plt
import numpy as np
physicsClient = p.connect(p.GUI)
p.setGravity(0,0,0)
bearStartPos1 = [-3.3,0,0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
useRealTimeSimulation = 1
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
camera = p.getDebugVisualizerCamera()
viewMat = camera[2]
projMat = camera[3]
#An example of setting the view matrix for the projective texture.
#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
def __init_bullet(self):
# Simulation world setup
self._physics_client = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
id_plane = p.loadURDF("plane.urdf")
import pybullet as p
p.connect(p.SHARED_MEMORY)
objects = [
p.loadURDF("plane.urdf", 0.000000, 0.000000, -.300000, 0.000000, 0.000000, 0.000000, 1.000000)
]
objects = [
p.loadURDF("quadruped/minitaur.urdf", [-0.000046, -0.000068, 0.200774],
[-0.000701, 0.000387, -0.000252, 1.000000],
useFixedBase=False)
]
ob = objects[0]
jointPositions = [
0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000,
-2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193,
0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517
]
for ji in range(p.getNumJoints(ob)):
p.resetJointState(ob, ji, jointPositions[ji])
p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0)
cid0 = p.createConstraint(1, 3, 1, 6, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid0, maxForce=500.000000)
cid1 = p.createConstraint(1, 16, 1, 19, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid1, maxForce=500.000000)
cid2 = p.createConstraint(1, 9, 1, 12, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
import pybullet as p
usePort = True
if (usePort):
id = p.connect(p.GRPC,"localhost:12345")
else:
id = p.connect(p.GRPC,"localhost")
print("id=",id)
if (id<0):
print("Cannot connect to GRPC server")
exit(0)
print ("Connected to GRPC")
r2d2 = p.loadURDF("r2d2.urdf")
print("numJoints = ", p.getNumJoints(r2d2))
def __init__(self, urdf_root=pybullet_data.getDataPath(), renders=False, is_discrete=True,
name="mobile_robot", max_distance=1.6, shape_reward=False, record_data=False, srl_model="raw_pixels",
random_target=False, force_down=True, state_dim=-1, learn_states=False, verbose=False,
save_path='srl_zoo/data/', env_rank=0, srl_pipe=None, fpv=False, **_):
super(MobileRobotGymEnv, self).__init__(srl_model=srl_model,
relative_pos=RELATIVE_POS,
env_rank=env_rank,
srl_pipe=srl_pipe)
self._timestep = 1. / 240.
self._urdf_root = urdf_root
self._observation = []
self._env_step_counter = 0
self._renders = renders
self._width = RENDER_WIDTH
self._height = RENDER_HEIGHT
self._cam_dist = 4.4
self._cam_yaw = 90
self._cam_pitch = -90
self._cam_roll = 0
self._max_distance = max_distance
self._shape_reward = shape_reward
self._random_target = random_target
self._force_down = force_down
self.camera_target_pos = (2, 2, 0)
self._is_discrete = is_discrete
self.terminated = False
self.renderer = p.ER_TINY_RENDERER
self.debug = False
self.n_contacts = 0
self.state_dim = state_dim
self.relative_pos = RELATIVE_POS
self.cuda = th.cuda.is_available()
self.saver = None
self.verbose = verbose
self.max_steps = MAX_STEPS
self.robot_pos = np.zeros(3)
self.robot_uid = None
self.target_pos = np.zeros(3)
self.target_uid = None
# Boundaries of the square env
self._min_x, self._max_x = 0, 4
self._min_y, self._max_y = 0, 4
self.has_bumped = False # Used for handling collisions
self.collision_margin = 0.1
self.walls = None
self.fpv = fpv
self.srl_model = srl_model
if record_data:
self.saver = EpisodeSaver(name, max_distance, state_dim, globals_=getGlobals(), relative_pos=RELATIVE_POS,
learn_states=learn_states, path=save_path)
if self._renders:
client_id = p.connect(p.SHARED_MEMORY)
if client_id < 0:
p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
self.debug = True
# Debug sliders for moving the camera
self.x_slider = p.addUserDebugParameter("x_slider", -10, 10, self.camera_target_pos[0])
self.y_slider = p.addUserDebugParameter("y_slider", -10, 10, self.camera_target_pos[1])
self.z_slider = p.addUserDebugParameter("z_slider", -10, 10, self.camera_target_pos[2])
self.dist_slider = p.addUserDebugParameter("cam_dist", 0, 10, self._cam_dist)
self.yaw_slider = p.addUserDebugParameter("cam_yaw", -180, 180, self._cam_yaw)
self.pitch_slider = p.addUserDebugParameter("cam_pitch", -180, 180, self._cam_pitch)
else:
p.connect(p.DIRECT)
global CONNECTED_TO_SIMULATOR
CONNECTED_TO_SIMULATOR = True
if self._is_discrete:
self.action_space = spaces.Discrete(N_DISCRETE_ACTIONS)
else:
self.action_space = spaces.Box(low=-1, high=1, shape=(2,), dtype=np.float32)
if self.srl_model == "ground_truth":
self.state_dim = self.getGroundTruthDim()
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:
self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(self.state_dim,), dtype=np.float32)
import pybullet as p
import pybullet_data as pd
import time
import math
usePhysX = True
useMaximalCoordinates = True
if usePhysX:
p.connect(p.PhysX,options="--numCores=8 --solver=pgs")
p.loadPlugin("eglRendererPlugin")
else:
p.connect(p.GUI)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.01)
p.setAdditionalSearchPath(pd.getDataPath())
#Always make ground plane maximal coordinates, to avoid performance drop in PhysX
#See https://github.com/NVIDIAGameWorks/PhysX/issues/71
p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json")
jran = 50
iran = 100
num=64
radius=0.1
numDominoes=0
def __init__(self, useFixedBase=False, useStairs=True, resetFunc=None):
# Simulation Configuration
self.useMaximalCoordinates = False
self.resetFunc = resetFunc
self.useRealTime = True
self.debugLidar = False
self.rotateCamera = False
self.debug = False
self.fixedTimeStep = 1. / 550
self.numSolverIterations = 200
self.useFixedBase = useFixedBase
self.useStairs = useStairs
self.init_oritentation = p.getQuaternionFromEuler([0, 0, 90.0])
self.init_position = [0, 0, 0.3]
self.reflection = False
self.state = RobotState.OFF
# Parameters for Servos - still wrong
self.kp = 0.045 #0.012
self.kd = .4 #.2
self.maxForce = 25.0
self.angles = [0.0, 0.0, 0.0, \
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]
self.physId = p.connect(p.SHARED_MEMORY)
if (self.physId < 0):
p.connect(p.GUI)
self.angle = 90
self.oldTextId = 0
self.textId = 0
self.oldDebugInfo = []
self.rot = (0, 0, 0)
self.pos = (0, 0, 0)
self.t = 0
if self.reflection:
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_reflection, 1)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 1)
self.IDkp = p.addUserDebugParameter("Kp", 0, 0.05, self.kp) # 0.05
self.IDkd = p.addUserDebugParameter("Kd", 0, 1, self.kd) # 0.5
self.IDmaxForce = p.addUserDebugParameter("MaxForce", 0, 50, 12.5)
p.setRealTimeSimulation(self.useRealTime)
self.quadruped = self.loadModels()
self.createEnv()
self.changeDynamics(self.quadruped)
self.jointNameToId = self.getJointNames(self.quadruped)
replaceLines = True
self.numRays = 360
self.rayFrom = []
self.rayTo = []
self.rayIds = []
self.rayHitColor = [1, 0, 0]
self.rayMissColor = [0, 1, 0]
rayLen = 12
rayStartLen = 0.12
for i in range(self.numRays):
#rayFrom.append([0,0,0])
h = 0.045
self.rayFrom.append([
rayStartLen * math.sin(math.pi * 2 * float(i) / self.numRays),
rayStartLen * math.cos(math.pi * 2 * float(i) / self.numRays),
h
])
self.rayTo.append([
rayLen * math.sin(math.pi * 2 * float(i) / self.numRays),
rayLen * math.cos(math.pi * 2 * float(i) / self.numRays), h
])
if self.debugLidar:
if (replaceLines):
self.rayIds.append(
p.addUserDebugLine(
self.rayFrom[i],
self.rayTo[i],
self.rayMissColor,
parentObjectUniqueId=self.quadruped,
parentLinkIndex=self.jointNameToId["base_lidar"]))
else:
self.rayIds.append(-1)
self.L = 140
self.W = 75 + 5 + 40
self.dirs = [[-1, 1, 1], [1, 1, 1], [-1, 1, 1], [1, 1, 1]]
self.roll = 0
self.Lp = np.array([[120, -100, self.W / 2, 1],
[120, -100, -self.W / 2, 1],
[-50, -100, self.W / 2, 1],
[-50, -100, -self.W / 2, 1]])
self.kin = Kinematic()
p.setPhysicsEngineParameter(
numSolverIterations=self.numSolverIterations)
#p.setTimeStep(self.fixedTimeStep)
p.setRealTimeSimulation(self.useRealTime)
self.ref_time = time.time()
p.getCameraImage(320, 200) #160,100)
p.resetDebugVisualizerCamera(1, 85.6, 0, [-0.61, 0.12, 0.25])
# Camera Settings
fov, aspect, nearplane, farplane = 90, 1.3, .0111, 100
self.projection_matrix = p.computeProjectionMatrixFOV(
fov, aspect, nearplane, farplane)
self.lastLidarTime = 0
import pybullet as p
p.connect(p.GUI)
cube = p.loadURDF("cube.urdf")
frequency = 240
timeStep = 1. / frequency
p.setGravity(0, 0, -9.8)
p.changeDynamics(cube, -1, linearDamping=0, angularDamping=0)
p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
for i in range(frequency):
p.stepSimulation()
pos, orn = p.getBasePositionAndOrientation(cube)
print(pos)
def setup_simulation(self):
""" Setup the simulation. """
########## PYBULLET SETUP ##########
if self.record_movie and self.gui == p.GUI:
p.connect(self.gui,
options=('--background_color_red={}'
' --background_color_green={}'
' --background_color_blue={}'
' --mp4={}'
' --mp4fps={}').format(
self.vis_options_background_color_red,
self.vis_options_background_color_green,
self.vis_options_background_color_blue,
self.movie_name,
int(self.movie_speed / self.time_step)))
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)
elif self.gui == p.GUI:
p.connect(self.gui,
options=(' --background_color_red={}'
' --background_color_green={}'
' --background_color_blue={}').format(
self.vis_options_background_color_red,
self.vis_options_background_color_green,
self.vis_options_background_color_blue))
else:
p.connect(self.gui)
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Everything should fall down
p.setGravity(*[g * self.units.gravity for g in self.gravity])
p.setPhysicsEngineParameter(fixedTimeStep=self.time_step,
numSolverIterations=self.solver_iterations,
numSubSteps=self.num_substep,
solverResidualThreshold=1e-10,
erp=0.0,
contactERP=self.contactERP,
frictionERP=0.0,
globalCFM=self.globalCFM,
reportSolverAnalytics=1)
# Turn off rendering while loading the models
self.rendering(0)
if self.ground == 'floor':
# Add floor
self.plane = p.loadURDF('plane.urdf', [0, 0, -0.],
globalScaling=0.01 * self.units.meters)
# When plane is used the link id is -1
self.link_plane = -1
p.changeDynamics(self.plane,
-1,
lateralFriction=self.ground_friction_coef)
self.sim_data.add_table('base_linear_velocity')
self.sim_data.add_table('base_angular_velocity')
self.sim_data.add_table('base_orientation')
elif self.ground == 'ball':
# Add floor and ball
self.floor = p.loadURDF('plane.urdf', [0, 0, -0.],
globalScaling=0.01 * self.units.meters)
if self.ball_info:
self.ball_radius, ball_pos = self.load_ball_info()
else:
ball_pos = None
self.plane = self.add_ball(self.ball_radius, self.ball_density,
self.ball_mass,
self.ground_friction_coef, ball_pos)
# When ball is used the plane id is 2 as the ball has 3 links
self.link_plane = 2
self.sim_data.add_table('ball_rotations')
self.sim_data.add_table('ball_velocity')
# Add the animal model
if '.sdf' in self.model and self.behavior is not None:
self.animal, self.link_id, self.joint_id = load_sdf(
self.model, force_concave=self.enable_concave_mesh)
elif '.sdf' in self.model and self.behavior is None:
self.animal = p.loadSDF(self.model)[0]
# Generate joint_name to id dict
self.link_id[p.getBodyInfo(self.animal)[0].decode('UTF-8')] = -1
for n in range(p.getNumJoints(self.animal)):
info = p.getJointInfo(self.animal, n)
_id = info[0]
joint_name = info[1].decode('UTF-8')
link_name = info[12].decode('UTF-8')
_type = info[2]
self.joint_id[joint_name] = _id
self.joint_type[joint_name] = _type
self.link_id[link_name] = _id
elif '.urdf' in self.model:
self.animal = p.loadURDF(self.model)
p.resetBasePositionAndOrientation(
self.animal, self.model_offset,
p.getQuaternionFromEuler([0., 0., 0.]))
self.num_joints = p.getNumJoints(self.animal)
# Body colors
color_wings = [91 / 100, 96 / 100, 97 / 100, 0.7]
color_eyes = [67 / 100, 21 / 100, 12 / 100, 1]
self.color_body = [140 / 255, 100 / 255, 30 / 255, 1]
self.color_legs = [170 / 255, 130 / 255, 50 / 255, 1]
self.color_collision = [0, 1, 0, 1]
nospecular = [0.5, 0.5, 0.5]
# Color the animal
p.changeVisualShape(self.animal,
-1,
rgbaColor=self.color_body,
specularColor=nospecular)
for link_name, _id in self.joint_id.items():
if 'Wing' in link_name and 'Fake' not in link_name:
p.changeVisualShape(self.animal, _id, rgbaColor=color_wings)
elif 'Eye' in link_name and 'Fake' not in link_name:
p.changeVisualShape(self.animal, _id, rgbaColor=color_eyes)
# and 'Fake' not in link_name:
elif ('Tarsus' in link_name or 'Tibia' in link_name
or 'Femur' in link_name or 'Coxa' in link_name):
p.changeVisualShape(self.animal,
_id,
rgbaColor=self.color_legs,
specularColor=nospecular)
elif 'Fake' not in link_name:
p.changeVisualShape(self.animal,
_id,
rgbaColor=self.color_body,
specularColor=nospecular)
# print('Link name {} id {}'.format(link_name, _id))
# Configure contacts
# Disable/Enable all self-collisions
for link0 in self.link_id.keys():
for link1 in self.link_id.keys():
p.setCollisionFilterPair(
bodyUniqueIdA=self.animal,
bodyUniqueIdB=self.animal,
linkIndexA=self.link_id[link0],
linkIndexB=self.link_id[link1],
enableCollision=0,
)
# Disable/Enable tarsi-ground collisions
for link in self.link_id.keys():
if 'Tarsus' in link:
p.setCollisionFilterPair(bodyUniqueIdA=self.animal,
bodyUniqueIdB=self.plane,
linkIndexA=self.link_id[link],
linkIndexB=self.link_plane,
enableCollision=1)
# Disable/Enable selected self-collisions
for (link0, link1) in self.self_collisions:
p.setCollisionFilterPair(
bodyUniqueIdA=self.animal,
bodyUniqueIdB=self.animal,
linkIndexA=self.link_id[link0],
linkIndexB=self.link_id[link1],
enableCollision=1,
)
# ADD container columns
# ADD ground reaction forces and friction forces
_ground_sensor_ids = []
for contact in self.ground_contacts:
_ground_sensor_ids.append((self.animal, self.plane,
self.link_id[contact], self.link_plane))
self.sim_data.contact_flag.add_parameter(f"{contact}_flag")
for axis in ('x', 'y', 'z'):
self.sim_data.contact_position.add_parameter(contact + '_' +
axis)
self.sim_data.contact_normal_force.add_parameter(contact +
'_' + axis)
self.sim_data.contact_lateral_force.add_parameter(contact +
'_' + axis)
# ADD self collision forces
_collision_sensor_ids = []
for link0, link1 in self.self_collisions:
_collision_sensor_ids.append(
(self.animal, self.animal, self.link_id[link0],
self.link_id[link1]))
contacts = '-'.join((link0, link1))
for axis in ['x', 'y', 'z']:
self.sim_data.contact_position.add_parameter(contacts + '_' +
axis)
self.sim_data.contact_normal_force.add_parameter(contacts +
'_' + axis)
self.sim_data.contact_lateral_force.add_parameter(contacts +
'_' + axis)
# Generate sensors
self.joint_sensors = JointSensors(self.animal,
self.sim_data,
meters=self.units.meters,
velocity=self.units.velocity,
torques=self.units.torques)
self.contact_sensors = ContactSensors(
self.sim_data,
tuple([*_ground_sensor_ids, *_collision_sensor_ids]),
meters=self.units.meters,
newtons=self.units.newtons)
self.com_sensor = COMSensor(self.animal,
self.sim_data,
meters=self.units.meters,
kilograms=self.units.kilograms)
# ADD base position parameters
for axis in ['x', 'y', 'z']:
self.sim_data.base_position.add_parameter(f'{axis}')
# self.sim_data.thorax_force.add_parameter(f'{axis}')
if self.ground == 'ball':
self.sim_data.ball_rotations.add_parameter(f'{axis}')
self.sim_data.ball_velocity.add_parameter(f'{axis}')
if self.ground == 'floor':
self.sim_data.base_linear_velocity.add_parameter(f'{axis}')
self.sim_data.base_angular_velocity.add_parameter(f'{axis}')
self.sim_data.base_orientation.add_parameter(f'{axis}')
# ADD joint parameters
for name, _ in self.joint_id.items():
self.sim_data.joint_positions.add_parameter(name)
self.sim_data.joint_velocities.add_parameter(name)
self.sim_data.joint_torques.add_parameter(name)
# ADD Center of mass parameters
for axis in ('x', 'y', 'z'):
self.sim_data.center_of_mass.add_parameter(f"{axis}")
# ADD muscles
if self.use_muscles:
self.initialize_muscles()
# ADD controller
if self.controller_config:
self.controller = NeuralSystem(
config_path=self.controller_config,
container=self.container,
)
# Disable default bullet controllers
p.setJointMotorControlArray(self.animal,
np.arange(self.num_joints),
p.VELOCITY_CONTROL,
targetVelocities=np.zeros(
(self.num_joints, 1)),
forces=np.zeros((self.num_joints, 1)))
p.setJointMotorControlArray(self.animal,
np.arange(self.num_joints),
p.POSITION_CONTROL,
forces=np.zeros((self.num_joints, 1)))
p.setJointMotorControlArray(self.animal,
np.arange(self.num_joints),
p.TORQUE_CONTROL,
forces=np.zeros((self.num_joints, 1)))
# Disable link linear and angular damping
for njoint in range(self.num_joints):
p.changeDynamics(self.animal, njoint, linearDamping=0.0)
p.changeDynamics(self.animal, njoint, angularDamping=0.0)
p.changeDynamics(self.animal, njoint, jointDamping=0.0)
self.total_mass = 0.0
for j in np.arange(-1, p.getNumJoints(self.animal)):
self.total_mass += p.getDynamicsInfo(self.animal,
j)[0] / self.units.kilograms
self.bodyweight = -1 * self.total_mass * self.gravity[2]
print('Total mass = {}'.format(self.total_mass))
if self.gui == p.GUI:
self.rendering(1)
#script to control a simulated robot hand using a VR glove
#see https://twitter.com/erwincoumans/status/821953216271106048
#and https://www.youtube.com/watch?v=I6s37aBXbV8
#vr glove was custom build using Spectra Symbolflex sensors (4.5")
#inside a Under Armour Batting Glove, using DFRobot Bluno BLE/Beetle
#with BLE Link to receive serial (for wireless bluetooth serial)
import serial
import time
import pybullet as p
#first try to connect to shared memory (VR), if it fails use local GUI
c = p.connect(p.SHARED_MEMORY)
if (c<0):
c = p.connect(p.GUI)
p.setInternalSimFlags(0)#don't load default robot assets etc
p.resetSimulation()
#p.resetSimulation()
p.setGravity(0,0,-10)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
# R = np.sqrt(X**2 + Y**2)
# Z = np.sin(R)
# # Plot the surface.
# surf = ax.plot_surface(X, Y, Z)
# plt.show()
# convert to an obj file
#### for pybullet now ##########
# set up the view
physicsClient = p.connect(p.GUI) # turn on simulation
#physicsClient = p.connect(p.DIRECT) # just print to command line
# ability to get files from the intenet that are not downloaded locally :)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = p.loadURDF('plane.urdf')
planeId = p.loadURDF('kuka_iiwa/model.urdf')
# # make a visual shape id
# visualShapeId = p.createVisualShape(
# shapeType=p.GEOM_MESH,
# fileName='plant.obj',
# rgbaColor=None,
# meshScale=[10e-4, 10e-4, 10e-4])
# # create a collision shape id
def save_traj_from_pt(args):
if use_gpu:
torch.backends.cudnn.deterministic = True
print(colored("Using CUDA.", p_color))
torch.cuda.manual_seed(args.seed)
torch.manual_seed(args.seed)
np.random.seed(args.seed)
random.seed(args.seed)
""" Create environment and get environment's info. """
if args.env_id > 10 and args.env_id <= 20:
import pybullet
import pybullet_envs
pybullet.connect(pybullet.DIRECT)
env = gym.make(args.env_name)
env.seed(args.seed)
if args.render:
env.render(mode="human")
state_dim = env.observation_space.shape[0]
is_disc_action = args.env_discrete
action_dim = (0 if is_disc_action else env.action_space.shape[0])
if is_disc_action:
a_bound = 1
action_num = env.action_space.n
print("State dim: %d, action num: %d" % (state_dim, action_num))
else:
args.a_bound_pre = np.asscalar(env.action_space.high[0])
""" always normalize env. """
from my_utils.my_gym_utils import NormalizeGymWrapper
env = NormalizeGymWrapper(env)
a_bound = np.asscalar(env.action_space.high[0])
a_low = np.asscalar(env.action_space.low[0])
assert a_bound == -a_low
print("State dim: %d, action dim: %d, action bound %d" %
(state_dim, action_dim, a_bound))
""" Set method and hyper parameter in file name"""
method_name = args.rl_method.upper()
hypers = rl_hypers_parser(args)
exp_name = "%s-%s_s%d" % (method_name, hypers, args.seed)
""" Set path for trajectory files """
traj_path = "../imitation_data/TRAJ_h5/%s/" % (args.env_name)
pathlib.Path(traj_path).mkdir(parents=True, exist_ok=True)
print("%s trajectory will be saved at %s" %
(colored(method_name, p_color), colored(traj_path, p_color)))
"""define actor and critic"""
if is_disc_action:
if args.rl_method == "dqn":
policy_updater = DQN(state_dim=state_dim,
action_num=action_num,
args=args)
if args.rl_method == "qr_dqn":
policy_updater = QR_DQN(state_dim=state_dim,
action_num=action_num,
args=args)
if args.rl_method == "clipped_ddqn":
policy_updater = Clipped_DDQN(state_dim=state_dim,
action_num=action_num,
args=args)
if args.rl_method == "ppo":
policy_updater = PPO(state_dim=state_dim,
action_dim=action_dim,
args=args,
a_bound=action_num,
is_discrete=True)
else:
if args.rl_method == "ac":
policy_updater = AC(state_dim=state_dim,
action_dim=action_dim,
args=args,
a_bound=a_bound)
if args.rl_method == "sac":
policy_updater = SAC(state_dim=state_dim,
action_dim=action_dim,
args=args,
a_bound=a_bound)
if args.rl_method == "td3":
policy_updater = TD3(state_dim=state_dim,
action_dim=action_dim,
args=args,
a_bound=a_bound)
if args.rl_method == "trpo":
policy_updater = TRPO(state_dim=state_dim,
action_dim=action_dim,
args=args,
a_bound=a_bound)
if args.rl_method == "ppo":
policy_updater = PPO(state_dim=state_dim,
action_dim=action_dim,
args=args,
a_bound=a_bound)
""" Load policy """
load_step = args.demo_iter
print(load_step)
model_path = "../imitation_data/RL_models/%s/%s-%s" % (
args.env_name, args.env_name, exp_name)
model_filename = model_path + ("_policy_T%d.pt" % (load_step))
policy_updater.load_model(model_filename)
policy_updater.policy_to_device(device_cpu)
print("RL model is loaded from %s" % model_filename)
for noise_level in args.noise_level_list:
""" Reset seed again to make sure all noises use the same transition's RNG """
if use_gpu:
torch.cuda.manual_seed(args.seed)
torch.backends.cudnn.deterministic = True
torch.manual_seed(args.seed)
np.random.seed(args.seed)
random.seed(args.seed)
env.seed(args.seed)
""" Storages and counters """
expert_state_list = []
expert_action_list = []
expert_mask_list = []
expert_reward_list = []
total_step, avg_reward_episode = 0, 0
for i_episode in count():
state = env.reset()
reward_episode = 0
if "TN" in args.noise_type:
noise_model = TN(env.action_space,
mu=0.0,
theta=0.15,
max_sigma=noise_level,
min_sigma=0.01,
decay_period=1000)
for t in range(0, args.t_max):
state_var = torch.FloatTensor(state)
if args.traj_deterministic:
action = policy_updater.greedy_action(
torch.FloatTensor(state).unsqueeze(0)).to(
device_cpu).detach().numpy()
else:
action = policy_updater.sample_action(
torch.FloatTensor(state).unsqueeze(0)).to(
device_cpu).detach().numpy()
# add noise to action
if args.noise_type == "normal" and noise_level > 0:
action = action + np.random.normal(0, noise_level,
action.shape)
elif args.noise_type == "SDNTN" and noise_level > 0:
action = action + noise_model.get_noise(t) * norm(
action, ord=1) / action_dim
# action = np.clip(action, a_min=a_low, a_max=a_bound)
next_state, reward, done, _ = env.step(
np.clip(action, a_min=a_low, a_max=a_bound))
if args.render:
env.render("human")
time.sleep(0.001)
if t + 1 == args.t_max:
done = 1
expert_state_list.append(state)
expert_action_list.append(action)
expert_mask_list.append(int(not done))
expert_reward_list.append(reward)
state = next_state
total_step += 1
reward_episode += reward
if done:
break
avg_reward_episode += reward_episode
if i_episode % 10 == 0:
print('Episode {}\t reward: {:.2f}'.format(
i_episode, reward_episode))
if total_step >= args.demo_file_size:
break
expert_states = np.array(expert_state_list)
expert_actions = np.array(expert_action_list)
expert_masks = np.array(expert_mask_list)
expert_rewards = np.array(expert_reward_list)
print("Total steps %d, total episode %d, AVG reward: %f" %
(total_step, i_episode + 1, avg_reward_episode /
(i_episode + 1)))
print(expert_actions.min())
print(expert_actions.max())
""" save data """
traj_filename = traj_path + (
"/%s_TRAJ-N%d_%s%0.2f" %
(args.env_name, args.demo_file_size, args.noise_type, noise_level))
if args.traj_deterministic:
traj_filename += "_det"
hf = h5py.File(traj_filename + ".h5", 'w')
hf.create_dataset('expert_source_path',
data=model_filename) # network model file.
hf.create_dataset('expert_states', data=expert_states)
hf.create_dataset('expert_actions', data=expert_actions)
hf.create_dataset('expert_masks', data=expert_masks)
hf.create_dataset('expert_rewards', data=expert_rewards)
print("TRAJ result is saved at %s" % traj_filename)
pos.append((x, y, z))
x += 2.0 * rad
if x > xymax:
x = xymin + (x - xymax)
y += 2.0 * rad
if y > xymax:
y = xymin + (y - xymax)
z += 2.0 * rad
for p in pos:
ob = pb.loadURDF("sphere_1cm.urdf",
basePosition=p,
useMaximalCoordinates=True)
if __name__ == '__main__':
pb.connect(pb.GUI, options="--opengl2")
#pb.connect(pb.DIRECT)
pb.setAdditionalSearchPath(pybullet_data.getDataPath())
pb.setInternalSimFlags(0)
pb.resetSimulation()
#pb.configureDebugVisualizer(pb.COV_ENABLE_WIREFRAME ,1)
pb.configureDebugVisualizer(pb.COV_ENABLE_SHADOWS, 0)
pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 1)
pb.configureDebugVisualizer(pb.COV_ENABLE_TINY_RENDERER, 0)
pb.configureDebugVisualizer(pb.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
pb.configureDebugVisualizer(pb.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
pb.configureDebugVisualizer(pb.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
import matplotlib.pyplot as plt
import pybullet
import time
import numpy as np #to reshape for matplotlib
plt.ion()
img = [[1,2,3]*50]*100#np.random.rand(200, 320)
#img = [tandard_normal((50,100))
image = plt.imshow(img,interpolation='none',animated=True,label="blah")
ax = plt.gca()
pybullet.connect(pybullet.DIRECT)
#pybullet.loadPlugin("eglRendererPlugin")
pybullet.loadURDF("plane.urdf",[0,0,-1])
pybullet.loadURDF("r2d2.urdf")
pybullet.setGravity(0,0,-10)
camTargetPos = [0,0,0]
cameraUp = [0,0,1]
cameraPos = [1,1,1]
pitch = -10.0
roll=0
upAxisIndex = 2
camDistance = 4
import pybullet as p
import time
p.connect(p.GUI)
planeUidA = p.loadURDF("plane_transparent.urdf", [0, 0, 0])
planeUid = p.loadURDF("plane_transparent.urdf", [0, 0, -1])
texUid = p.loadTexture("tex256.png")
p.changeVisualShape(planeUidA, -1, rgbaColor=[1, 1, 1, 0.5])
p.changeVisualShape(planeUid, -1, rgbaColor=[1, 1, 1, 0.5])
p.changeVisualShape(planeUid, -1, textureUniqueId=texUid)
width = 256
height = 256
pixels = [255] * width * height * 3
colorR = 0
colorG = 0
colorB = 0
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
#p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
blue = 0
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,
"renderbench.json")
for i in range(100000):
p.stepSimulation()
for i in range(width):
for j in range(height):
pixels[(i + j * width) * 3 + 0] = i
pixels[(i + j * width) * 3 + 1] = (j + blue) % 256
import pybullet as p
import time
#p.connect(p.UDP,"192.168.86.100")
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)]
pr2_gripper = objects[0]
print ("pr2_gripper=")
print (pr2_gripper)
jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
for jointIndex in range (p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
print ("pr2_cid")
print (pr2_cid)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
def __init__(self):
"""
Initialise the environment
"""
self.random_goal = True
self.goal_oriented = True
self.obs_type = 1
self.reward_type = 1
self.action_type = 1
# Define action space
self.action_space = spaces.Box(
low=np.float32(
np.array([-0.5, -0.25, -0.25, -0.25, -0.5, -0.005]) / 30),
high=np.float32(
np.array([0.5, 0.25, 0.25, 0.25, 0.5, 0.005]) / 30),
dtype=np.float32)
# Define observation space
if self.obs_type == 1:
self.obs_space_low = np.float32(
np.concatenate((MIN_END_EFF_COORDS, JOINT_MIN), axis=0))
self.obs_space_high = np.float32(
np.concatenate((MAX_END_EFF_COORDS, JOINT_MAX), axis=0))
elif self.obs_type == 2:
self.obs_space_low = np.float32(
np.concatenate((MIN_GOAL_COORDS, JOINT_MIN), axis=0))
self.obs_space_high = np.float32(
np.concatenate((MAX_GOAL_COORDS, JOINT_MAX), axis=0))
elif self.obs_type == 3:
self.obs_space_low = np.float32(
np.concatenate(([-1.0] * 6, JOINT_MIN), axis=0))
self.obs_space_high = np.float32(
np.concatenate(([1.0] * 6, JOINT_MAX), axis=0))
elif self.obs_type == 4:
self.obs_space_low = np.float32(
np.concatenate(([-1.0] * 3, JOINT_MIN), axis=0))
self.obs_space_high = np.float32(
np.concatenate(([1.0] * 3, JOINT_MAX), axis=0))
elif self.obs_type == 5:
self.obs_space_low = np.float32(
np.concatenate(([-1.0] * 6, MIN_GOAL_COORDS, JOINT_MIN),
axis=0))
self.obs_space_high = np.float32(
np.concatenate(([1.0] * 6, MAX_GOAL_COORDS, JOINT_MAX),
axis=0))
self.observation_space = spaces.Box(low=self.obs_space_low,
high=self.obs_space_high,
dtype=np.float32)
if self.goal_oriented:
self.observation_space = spaces.Dict(
dict(desired_goal=spaces.Box(low=np.float32(MIN_GOAL_COORDS),
high=np.float32(MAX_GOAL_COORDS),
dtype=np.float32),
achieved_goal=spaces.Box(
low=np.float32(MIN_END_EFF_COORDS),
high=np.float32(MAX_END_EFF_COORDS),
dtype=np.float32),
observation=self.observation_space))
# Initialise goal position
if self.random_goal:
self.goal = self.sample_random_goal()
else:
self.goal = FIXED_GOAL_COORDS
# Connect to physics client. By default, do not render
self.physics_client = p.connect(p.DIRECT)
# Load URDFs
self.create_world()
# reset environment
self.reset()
import pybullet as p
draw = 1
printtext = 0
if (draw):
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
r2d2 = p.loadURDF("r2d2.urdf")
def drawAABB(aabb):
f = [aabbMin[0], aabbMin[1], aabbMin[2]]
t = [aabbMax[0], aabbMin[1], aabbMin[2]]
p.addUserDebugLine(f, t, [1, 0, 0])
f = [aabbMin[0], aabbMin[1], aabbMin[2]]
t = [aabbMin[0], aabbMax[1], aabbMin[2]]
p.addUserDebugLine(f, t, [0, 1, 0])
f = [aabbMin[0], aabbMin[1], aabbMin[2]]
t = [aabbMin[0], aabbMin[1], aabbMax[2]]
p.addUserDebugLine(f, t, [0, 0, 1])
f = [aabbMin[0], aabbMin[1], aabbMax[2]]
t = [aabbMin[0], aabbMax[1], aabbMax[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMin[0], aabbMin[1], aabbMax[2]]
t = [aabbMax[0], aabbMin[1], aabbMax[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMax[0], aabbMin[1], aabbMin[2]]
def render(self, mode='human'):
""" Render Pybullet simulation """
p.disconnect(self.physics_client)
self.physics_client = p.connect(p.GUI)
self.create_world()
