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 _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, mode='direct'):
"""Initializes the connection to Bullet.
:param mode: Mode of the connection. Options: gui | direct
:type mode: str
"""
while True:
if pb.getConnectionInfo(self.__client_id)['isConnected'] > 0:
self.__client_id += 1
else:
break
self.physicsClient = pb.connect({
'gui': pb.GUI,
'direct': pb.DIRECT
}[mode], self.__client_id) #or p.DIRECT for non-graphical version
pb.setGravity(*self.gravity, physicsClientId=self.__client_id)
pb.setTimeStep(self.time_step, physicsClientId=self.__client_id)
def plot_test_set():
# plot test data angles and positions
for (i, point) in enumerate(test_data[:10]):
bb = BusyBox.bb_from_result(point)
if p.getConnectionInfo()['isConnected']:
p.disconnect()
setup_pybullet.setup_env(bb, False, False)
mech = bb._mechanisms[0]
true_policy = policies.generate_policy(mech, False)
pos = (true_policy.rigid_position[0],
true_policy.rigid_position[2])
pos_ax.plot(*pos, 'm.')
pos_ax.annotate(i, pos)
if i == len(test_data):
angle_ax.plot(true_policy.pitch, 2.0, 'k.', label='test data')
else:
angle_ax.plot(true_policy.pitch, 2.0, 'k.')
angle_ax.annotate(i, (true_policy.pitch, 2.0))
parser.initializeFromBulletBody(mb, physicsClientId=org)
parser.saveUrdf("test.urdf")
if (1):
print("\ncreatingMultiBody...................n")
obUid = parser.createMultiBody(physicsClientId=gui)
parser2 = UrdfEditor()
print("\n###########################################\n")
parser2.initializeFromBulletBody(obUid, physicsClientId=gui)
parser2.saveUrdf("test2.urdf")
for i in range(p.getNumJoints(obUid, physicsClientId=gui)):
p.setJointMotorControl2(obUid,
i,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=1,
physicsClientId=gui)
print(p.getJointInfo(obUid, i, physicsClientId=gui))
parser = 0
p.setRealTimeSimulation(1, physicsClientId=gui)
while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]):
p.stepSimulation(physicsClientId=gui)
time.sleep(0.01)
parser = urdfEditor.UrdfEditor()
parser.initializeFromBulletBody(mb,physicsClientId=org)
parser.saveUrdf("test.urdf")
if (1):
print("\ncreatingMultiBody...................n")
obUid = parser.createMultiBody(physicsClientId=gui)
parser2 = urdfEditor.UrdfEditor()
print("\n###########################################\n")
parser2.initializeFromBulletBody(obUid,physicsClientId=gui)
parser2.saveUrdf("test2.urdf")
for i in range (p.getNumJoints(obUid, physicsClientId=gui)):
p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0,force=1,physicsClientId=gui)
print(p.getJointInfo(obUid,i,physicsClientId=gui))
parser=0
p.setRealTimeSimulation(1,physicsClientId=gui)
while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]):
p.stepSimulation(physicsClientId=gui)
time.sleep(0.01)
def get_connection():
return p.getConnectionInfo()['connectionMethod']
def isconnected(self):
return p.getConnectionInfo(self.cid)['isConnected']
def setup_env(bb, viz, debug, show_im=False):
# disconnect if already connected (may want to change viz from False to True)
if p.getConnectionInfo()['isConnected']:
p.disconnect()
if not viz:
client = p.connect(p.DIRECT)
else:
client = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 0)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setRealTimeSimulation(0)
p.resetDebugVisualizerCamera(cameraDistance=.15,
cameraYaw=180,
cameraPitch=0,
cameraTargetPosition=(0., 0., bb.height / 2))
plane_id = p.loadURDF("plane.urdf")
model = p.loadURDF(bb.file_name, [0., -.3, 0.])
bb.set_mechanism_ids(model)
#p.setGravity(0, 0, -10)
maxForce = 0
mode = p.VELOCITY_CONTROL
bb.set_joint_control_mode(mode, maxForce)
# enable torque sensor
if bb._mechanisms[0].mechanism_type == 'Door':
p.enableJointForceTorqueSensor(bb._bb_id,
bb._mechanisms[0]._door_base_id, True)
# can change resolution and shadows with this call
view_matrix = p.computeViewMatrixFromYawPitchRoll(
distance=0.4,
yaw=180,
pitch=0,
roll=0,
upAxisIndex=2,
cameraTargetPosition=(0., 0., bb.height / 2))
aspect = 205. / 154.
nearPlane = 0.01
farPlane = 100
fov = 60
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane,
farPlane)
image_data_pybullet = p.getCameraImage(205,
154,
shadow=0,
renderer=p.ER_TINY_RENDERER,
viewMatrix=view_matrix,
lightDiffuseCoeff=0,
lightSpecularCoeff=0,
projectionMatrix=projection_matrix)
image_data = util.ImageData(*image_data_pybullet[:3])
w, h, im = image_data_pybullet[:3]
np_im = np.array(im, dtype=np.uint8).reshape(h, w, 4)[:, :, 0:3]
np_im = np_im[7:125, 44:160, :]
w, h, im = np_im.shape[1], np_im.shape[0], np_im.flatten().tolist()
image_data = util.ImageData(w, h, im)
# Display the cropped image.
if show_im:
np_im = np.array(im, dtype=np.uint8).reshape(h, w, 3)
plt.imshow(np_im)
plt.show()
p.stepSimulation()
gripper = Gripper(bb._mechanisms[0])
return image_data, gripper
def get_connection(client=None):
return p.getConnectionInfo(
physicsClientId=get_client(client))['connectionMethod']
def is_connected():
return p.getConnectionInfo(physicsClientId=CLIENT)['isConnected']
# Check Env properties
# Test World properties
# Check is_sim flag
assert env.world.is_sim
# Check observation space type
assert isinstance(env.observation_space, spaces.Box)
# Check action space type
assert isinstance(env.action_space, spaces.Box)
# Check use visualizer flag set.
assert env.world.use_visualizer
# Pybullet specific checks
# Check pybullet connection.
assert isinstance(env._physics_id, int)
assert pb.getConnectionInfo(env._physics_id) == {
'connectionMethod': 1,
'isConnected': 1
}
# Check object loading
assert env.has_objects
assert isinstance(env.object_interfaces, dict)
assert '013_apple' in env.object_interfaces
# BulletObjectInterface Tests
# Attributes
assert isinstance(env.object_interfaces['013_apple'].obj_id, int)
assert env.object_interfaces['013_apple'].physics_id == env._physics_id
# Object States
for link in self.urdfLinks:
self.writeLink(file, link)
for joint in self.urdfJoints:
self.writeJoint(file, joint)
file.write("</robot>\n")
file.close()
def __del__(self):
pass
parser = UrdfEditor()
parser.initializeFromBulletBody(door)
parser.saveUrdf("test.urdf")
parser = 0
p.setRealTimeSimulation(1)
print("numJoints:", p.getNumJoints(door))
print("base name:", p.getBodyInfo(door))
for i in range(p.getNumJoints(door)):
print("jointInfo(", i, "):", p.getJointInfo(door, i))
print("linkState(", i, "):", p.getLinkState(door, i))
while (p.getConnectionInfo()["isConnected"]):
time.sleep(0.01)
def viz_train_test_data(train_data, test_data):
import matplotlib.pyplot as plt
from gen.generator_busybox import BusyBox
n_inter_per_bb = 100
n_bbs = int(len(train_data) / n_inter_per_bb)
training_dataset_size = n_bbs #10
plt.ion()
angle_fig, angle_ax = plt.subplots()
pos_fig, pos_ax = plt.subplots()
def plot_test_set():
# plot test data angles and positions
for (i, point) in enumerate(test_data[:10]):
bb = BusyBox.bb_from_result(point)
if p.getConnectionInfo()['isConnected']:
p.disconnect()
setup_pybullet.setup_env(bb, False, False)
mech = bb._mechanisms[0]
true_policy = policies.generate_policy(mech, False)
pos = (true_policy.rigid_position[0],
true_policy.rigid_position[2])
pos_ax.plot(*pos, 'm.')
pos_ax.annotate(i, pos)
if i == len(test_data):
angle_ax.plot(true_policy.pitch, 2.0, 'k.', label='test data')
else:
angle_ax.plot(true_policy.pitch, 2.0, 'k.')
angle_ax.annotate(i, (true_policy.pitch, 2.0))
dataset_sizes = list(
range(training_dataset_size, n_bbs + 1, training_dataset_size))
pitches = []
# plot training data angles and positions
for n in range(1, n_bbs + 1):
point = train_data[(n - 1) * n_inter_per_bb]
if p.getConnectionInfo()['isConnected']:
p.disconnect()
bb = BusyBox.bb_from_result(point)
setup_pybullet.setup_env(bb, False, False)
mech = bb._mechanisms[0]
true_policy = policies.generate_policy(mech, False)
pitches += [true_policy.pitch]
pos = (true_policy.rigid_position[0], true_policy.rigid_position[2])
pos_ax.plot(*pos, 'c.')
if n in dataset_sizes:
angle_ax.clear()
pos_ax.set_title('Positions of Training and Test Data, n_bbs=' +
str(n))
pos_fig.savefig('dataset_imgs/poss_n_bbs' + str(n))
angle_ax.set_title('Historgram of Training Data Angle, n_bbs=' +
str(n))
angle_ax.hist(pitches, 30)
plot_test_set()
#angle_fig.savefig('dataset_imgs/pitches_n_bbs'+str(n))
plt.show()
input('enter to close')
angle_ax.set_title('Historgram of Training Data Angle, n_bbs=' +
str(n))
angle_ax.hist(pitches, 30)
plot_test_set()
#angle_fig.savefig('dataset_imgs/pitches_n_bbs'+str(n))
plt.show()
input('enter to close')
# Check Env properties
# Test World properties
# Check is_sim flag
assert env.world.is_sim
# Check observation space type
assert isinstance(env.observation_space, spaces.Box)
# Check action space type
assert isinstance(env.action_space, spaces.Box)
# Check use visualizer flag set.
assert env.world.use_visualizer
# Pybullet specific checks
# Check pybullet connection.
assert isinstance(env._physics_id, int)
assert pb.getConnectionInfo(env._physics_id) == {'connectionMethod': 1, 'isConnected': 1}
# Check object loading
assert env.world.has_object
assert isinstance(env.object_interfaces, dict)
assert '013_apple' in env.object_interfaces
# BulletObjectInterface Tests
# Attributes
assert isinstance(env.object_interfaces['013_apple'].obj_id, int)
assert env.object_interfaces['013_apple'].physics_id == env._physics_id
# Object States
assert len(env.object_interfaces['013_apple'].pose) == 7
assert len(env.object_interfaces['013_apple'].position) == 3
assert len(env.object_interfaces['013_apple'].orientation) == 4
def isconnected(self):
"""
:return: pybullet is alive
"""
return p.getConnectionInfo(self.cid)['isConnected']
def run(self, runtime, goal_position=[]):
if (not goal_position == []) and (not self.goal_position
== goal_position):
# if self.goalBodyID is not None:
# pybullet.removeBody(self.goalBodyID, physicsClientId=self.physicsClient)
self.goalBodyID = pybullet.createMultiBody(
baseMass=0.0,
baseInertialFramePosition=[0, 0, 0],
baseVisualShapeIndex=self.visualGoalShapeId,
basePosition=goal_position,
useMaximalCoordinates=True,
physicsClientId=self.physicsClient)
# print ("Goal information: ", self.goalBodyID)
self.__base_collision = False
self.__heightExceed = False
self.goal_position = goal_position
assert not self.__controller == None, "Controller not set"
self.__states = [self.getState()] #load with init state
self.__rotations = [self.getRotation()]
self.__commands = []
old_time = 0
first = True
self.__command = object
controlStep = 0
self.flip = False
for i in range(int(runtime / self.__simStep)):
if i * self.__simStep - old_time > self.__controlStep or first:
state = self.getState()
self.__command = self.__controller.nextCommand(i *
self.__simStep)
if not first:
self.__states.append(state)
self.__rotations.append(self.getRotation())
self.__commands.append(self.__command)
first = False
old_time = i * self.__simStep
controlStep += 1
self.set_commands(self.__command)
pybullet.stepSimulation(physicsClientId=self.physicsClient)
if pybullet.getConnectionInfo(
self.physicsClient)['connectionMethod'] == pybullet.GUI:
time.sleep(self.__simStep / float(self.__vspeed))
# Flipfing behavior
if self.getZOrientation() < 0.0:
self.flip = True
#Base floor collision
if len(
pybullet.getContactPoints(
self.__planeId,
self.__hexapodId,
-1,
-1,
physicsClientId=self.physicsClient)) > 0:
self.__base_collision = True
#Jumping behavior when CM crosses 2.2
if self.getState()[2] > 2.2:
self.__heightExceed = True
