def resetPybulletEnv(self):
# soft reset has bug in older pybullet versions. 2.7,1 works good
self.episode_count += 1
if self.episode_count % self.hard_reset_freq == 0:
self.initialize()
self.episode_count = 0
for o in self.objects:
pb.removeBody(o.object_id)
self.robot.reset()
self.objects = list()
self.object_types = {}
self.heightmap = None
self.current_episode_steps = 1
self.last_action = None
self.last_obj = None
self.state = {}
self.pb_state = None
while True:
try:
self._generateShapes(constants.RANDOM,
self.num_random_objects,
random_orientation=True)
except Exception as e:
continue
else:
break
pb.stepSimulation()
def reset(obj_id):
# remove if already exists
if obj_id is not None:
p.removeBody(obj_id)
# select rand file
rand_file = random.choice(model_list)
# load obj
print('Loading: ', rand_file)
# flags = p.URDF_INITIALIZE_SAT_FEATURES
obj_id = p.loadURDF(os.path.join(data_path, rand_file, "model.urdf"),
start_pos)
if auto_scale:
# Could use trimesh to get bounding box before loading in pybullet
# to avoid remove/reload
targ_diag = 0.15
obj_aabb = p.getAABB(obj_id)
aabb_min, aabb_max = obj_aabb[0], obj_aabb[1]
long_diag = np.linalg.norm(np.array(aabb_min) - np.array(aabb_max))
ratio = targ_diag / long_diag
p.removeBody(obj_id)
obj_id = p.loadURDF(
os.path.join(data_path, rand_file, "model.urdf"),
start_pos,
# flags=flags,
globalScaling=ratio)
return obj_id
def accelerate_backward(env, time=100, indicator_file=None):
"""
Accelerates forward for designated timesteps
"""
print("Accelerating backward for {} timesteps".format(time))
pos = env.robot.get_position()
# load initial indicator object
x0, y0, z0 = pos
if indicator_file is not None:
id = p.loadURDF(fileName=indicator_file,
basePosition=[x0, y0, z0 + 1],
useFixedBase=True)
# move robot while generating indicator object
for i in range(time):
pos = env.robot.get_position()
orn = env.robot.get_orientation()
old_x, y, z = pos
# generate indicator object at the begining
if i == 0 and indicator_file is not None:
p.resetBasePositionAndOrientation(bodyUniqueId=id,
posObj=[old_x, y, z + 1],
ornObj=orn)
if i == 20 and indicator_file is not None:
p.removeBody(id)
# move robot
ensure_orientation(env)
env.step(BACKWARD)
def _flag_reposition(self):
#self.walk_target_x = self.np_random.uniform(low=-self.scene.stadium_halflen,
# high=+self.scene.stadium_halflen)
#self.walk_target_y = self.np_random.uniform(low=-self.scene.stadium_halfwidth,
# high=+self.scene.stadium_halfwidth)
force_x = self.np_random.uniform(-300, 300)
force_y = self.np_random.uniform(-300, 300)
more_compact = 0.5 # set to 1.0 whole football field
#self.walk_target_x *= more_compact
#self.walk_target_y *= more_compact
startx, starty, _ = self.robot.get_position()
self.flag = None
#self.flag = self.scene.cpp_world.debug_sphere(self.walk_target_x, self.walk_target_y, 0.2, 0.2, 0xFF8080)
self.flag_timeout = 3000 / self.scene.frame_skip
#print('targetxy', self.flagid, self.walk_target_x, self.walk_target_y, p.getBasePositionAndOrientation(self.flagid))
#p.resetBasePositionAndOrientation(self.flagid, posObj = [self.walk_target_x, self.walk_target_y, 0.5], ornObj = [0,0,0,0])
if self.lastid:
p.removeBody(self.lastid)
self.lastid = p.createMultiBody(
baseMass=1,
baseVisualShapeIndex=self.visualid,
baseCollisionShapeIndex=self.colisionid,
basePosition=[startx, starty, 0.5])
p.applyExternalForce(self.lastid, -1, [force_x, force_y, 50],
[0, 0, 0], p.LINK_FRAME)
ball_xyz, _ = p.getBasePositionAndOrientation(self.lastid)
self.robot.walk_target_x = ball_xyz[0]
self.robot.walk_target_y = ball_xyz[1]
def reset(self):
global IDX
p.resetSimulation()
body_ids = p.loadMJCF('mjcf/point_mass.xml')
self._world_id = 0
self._goal_body_id = None
p.syncBodyInfo()
self.object_ids = []
for info in WALL_INFO:
self._load_box(info)
for info in OBSTACLE_INFO:
self._load_box(info)
self._setup_top_view()
p.setTimeStep(0.1)
dict_copy = {}
for key, val in GOAL.items():
dict_copy[key] = val
# x_min = 0.2
# x_max = 0.8
# xs = np.linspace(x_min, x_max, 10, endpoint=True)
dict_copy['x'] = 0.2
# IDX = (1 + IDX) % 10
self.goal_pos = np.asarray([dict_copy['x'], dict_copy['y']])
if self._goal_body_id is not None:
p.removeBody(self._goal_body_id)
self._load_goal(dict_copy)
rgb_img = self._get_top_view()
if self.img_obs:
obs = rgb_img
else:
obs = p.getLinkState(self._world_id, 1)[4][:2]
return obs
def remove(self, key):
obj_id = self._key_to_id[key]
pybullet.removeBody(obj_id, physicsClientId=self._physics_client)
self._key_to_id.pop(key)
self._key_to_com.pop(key)
if self._debug:
self._remove_from_scene(key)
def set_random_object_and_goal(self):
if self.itemId is not None:
p.removeBody(self.itemId)
if self.goalId is not None:
p.removeBody(self.goalId)
object_constraints = [(-0.24, 0.24), (-0.35, 0.35), 0.6567, 0, 0,
(-math.pi, math.pi)]
goal_constraints = [(-0.20, 0.20), (-0.25, 0.25), 0.633, 0, 0,
(-math.pi, math.pi)]
object_pose = self.random_pose(constraints=object_constraints)
#object_pose = [[-0.25, 0.35, 0.6567], [0, 0, math.pi/4]]
while True:
goal_pose = self.random_pose(constraints=goal_constraints)
dist_poses = Geometry.dist(goal_pose[0][:2], object_pose[0][:2])
if dist_poses > self.minGoalObj_dist:
break
if self.verbose:
print(object_pose)
print(goal_pose)
self.goalId = p.loadURDF('objects/goal/lego.urdf',
goal_pose[0],
p.getQuaternionFromEuler(goal_pose[1]),
globalScaling=3,
useFixedBase=True)
self.itemId = p.loadURDF('objects/lego/lego.urdf',
object_pose[0],
p.getQuaternionFromEuler(object_pose[1]),
globalScaling=3)
def __del__(self):
'''
This removes the bot out of the simulation
'''
for dockee in self.dockees:
res = self - dockee ## This would undock all the relations and remove on all sides
p.removeBody(self.base_id, self.pClient)
def remove_all_obstacles_by_id(obstacle_ids):
"""
Removes all obstacles from simulation
"""
for entry in obstacle_ids:
p.removeBody(entry)
return
def make_heightfield(self, height_map=None):
if self.config["terrain_name"] == "flat":
return
if hasattr(self, 'terrain'):
p.removeBody(self.terrain, physicsClientId=self.client_ID)
if height_map is None:
heightfieldData = np.zeros(self.config["env_width"] * self.config["max_steps"])
terrainShape = p.createCollisionShape(shapeType=p.GEOM_HEIGHTFIELD,
meshScale=[self.config["mesh_scale_lat"],
self.config["mesh_scale_lat"],
self.config["mesh_scale_vert"]],
heightfieldTextureScaling=(self.config["env_width"] - 1) / 2,
heightfieldData=heightfieldData,
numHeightfieldRows=self.config["max_steps"],
numHeightfieldColumns=self.config["env_width"],
physicsClientId=self.client_ID)
else:
heightfieldData = height_map.ravel(order='F')
terrainShape = p.createCollisionShape(shapeType=p.GEOM_HEIGHTFIELD,
meshScale=[self.config["mesh_scale_lat"],
self.config["mesh_scale_lat"],
self.config["mesh_scale_vert"]],
heightfieldTextureScaling=(self.config["env_width"] - 1) / 2,
heightfieldData=heightfieldData,
numHeightfieldRows=height_map.shape[0],
numHeightfieldColumns=height_map.shape[1],
physicsClientId=self.client_ID)
terrain = p.createMultiBody(0, terrainShape, physicsClientId=self.client_ID)
p.resetBasePositionAndOrientation(terrain, [0, 0, 0], [0, 0, 0, 1], physicsClientId=self.client_ID)
return terrain
def reset(self, force_randomize=None):
if hasattr(self, 'terrain'):
p.removeBody(self.terrain, physicsClientId=self.client_ID)
if hasattr(self, 'robot'):
p.removeBody(self.robot, physicsClientId=self.client_ID)
del self.robot
del self.terrain
del self.target_body
self.target = None
p.resetSimulation(physicsClientId=self.client_ID)
p.setGravity(0, 0, -9.8, physicsClientId=self.client_ID)
p.setRealTimeSimulation(0, physicsClientId=self.client_ID)
self.robot = self.load_robot()
if not self.config["terrain_name"] == "flat":
self.terrain = self.generate_rnd_env()
else:
self.terrain = p.loadURDF("plane.urdf", physicsClientId=self.client_ID)
self.create_targets()
# Reset episodal vars
self.step_ctr = 0
self.episode_ctr += 1
self.prev_yaw_dev = 0
# Get heightmap height at robot position
if self.terrain is None or self.config["terrain_name"] == "flat":
spawn_height = 0
else:
spawn_height = 0.5 * np.max(
self.terrain_hm[self.config["env_length"] // 2 - 3:self.config["env_length"] // 2 + 3,
self.config["env_width"] // 2 - 3: self.config["env_width"] // 2 + 3]) * self.config["mesh_scale_vert"]
# Random initial rotation
rnd_rot = np.random.rand() * 0.3 - 0.15
rnd_quat = p.getQuaternionFromAxisAngle([0, 0, 1], rnd_rot)
joint_init_pos_list = self.norm_to_rads([0] * 18)
[p.resetJointState(self.robot, i, joint_init_pos_list[i], 0, physicsClientId=self.client_ID) for i in range(18)]
p.resetBasePositionAndOrientation(self.robot, [0, 0, spawn_height + 0.15], rnd_quat,
physicsClientId=self.client_ID)
p.setJointMotorControlArray(bodyUniqueId=self.robot,
jointIndices=range(18),
controlMode=p.POSITION_CONTROL,
targetPositions=[0] * 18,
forces=[self.config["max_joint_force"]] * 18,
physicsClientId=self.client_ID)
self.update_targets()
self.prev_target_dist = np.sqrt((0 - self.target[0]) ** 2 + (0 - self.target[1]) ** 2)
tar_angle = np.arctan2(self.target[1] - 0, self.target[0] - 0)
self.prev_yaw_deviation = np.min((abs((rnd_rot % 6.283) - (tar_angle % 6.283)), abs(rnd_rot - tar_angle)))
for i in range(10):
p.stepSimulation(physicsClientId=self.client_ID)
obs, _, _, _ = self.step(np.zeros(self.act_dim))
return obs
def _set_size(self, pybullet_client_ids, position, orientation):
"""
:param pybullet_client_ids: (list) specifies the pybullet clients.
:param position: (list float) specifies the positions in x,y,z
:param orientation: (list float) specifies the quaternion of the goal.
:return: (list) the size of the bounding box of the object.
"""
temp_shape_id = pybullet.createCollisionShape(
shapeType=pybullet.GEOM_MESH,
meshScale=self._scale,
fileName=self._filename,
physicsClientId=pybullet_client_ids[0])
temp_block_id = pybullet.createMultiBody(
baseCollisionShapeIndex=temp_shape_id,
basePosition=position,
baseOrientation=orientation,
baseMass=0.1,
physicsClientId=pybullet_client_ids[0])
bb = pybullet.getAABB(temp_block_id,
physicsClientId=pybullet_client_ids[0])
size = np.array([(bb[1][0] - bb[0][0]), (bb[1][1] - bb[0][1]),
(bb[1][2] - bb[0][2])])
pybullet.removeBody(temp_block_id,
physicsClientId=pybullet_client_ids[0])
return size
def reset(self):
if hasattr(self, "block_id"):
for block in self.blocks:
pb.removeBody(self.block_id[block])
self.block_id = {}
self.blocks = []
super().reset()
def reset(self):
self.episode_count += 1
# TODO: objects sometimes don't get properly removed without hard resets
if self.episode_count >= 1:
self.initialize()
self.episode_count = 0
for o in self.objects:
pb.removeBody(o.object_id)
self.robot.reset()
self.objects = list()
self.object_types = {}
self.heightmap = None
self.current_episode_steps = 1
self.last_action = None
self.last_obj = None
self.state = {}
self.pb_state = None
while True:
try:
self._generateShapes(constants.RANDOM,
self.num_random_objects,
random_orientation=True)
except Exception as e:
continue
else:
break
pb.stepSimulation()
return self._getObservation()
def __init__(self, gui, urdf_dir):
self.sim_steps = 0
self.frame_num = 0
self.obj_ids = []
self.urdf_paths = glob.glob(f"{urdf_dir}/*/*.urdf")
print(self.urdf_paths)
if gui:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
# load table and arm
p.loadURDF(pybullet_data.getDataPath()+"/table/table.urdf", 0.5,0.,-0.82, 0,0,0,1)
self.kuka = kuka.Kuka(urdfRootPath=pybullet_data.getDataPath())
# remove tray (part of kuka env)
p.removeBody(self.kuka.trayUid)
# activate apple physics
p.setGravity(0, 0, -9.8)
#p.enableJointForceTorqueSensor(self.kuka.kukaUid, 8)
#p.enableJointForceTorqueSensor(self.kuka.kukaUid, 11)
self.left_finger_index = 8
self.left_fingertip_index = 10
self.right_finger_index = 11
self.right_fingertip_index = 13
def drop_objects(self, num_objects):
"""
Drop 'num_objects' objects onto surface. Objects are randomly picked from given urdf directory.
"""
for obj_uid in self.obj_ids:
p.removeBody(obj_uid)
self.obj_ids.clear()
# drop semi-random objects into tray
for _ in range(num_objects):
#random_obj_id = random.randint(0, 9)
#urdf_filename = "%s/%04d/%04d.urdf" % (obj_urdf_dir, random_obj_id, random_obj_id)
urdf_filename = np.random.choice(self.urdf_paths)
print("Dropping:", os.path.basename(urdf_filename))
# drop block fixed x distance from base of arm (0.51)
# across width of tray (-0.1, 0.3) and from fixed height (0.2)
block_pos = [u.random_in(0.51, 0.66), u.random_in(-0.1, 0.3), -0.1]
# drop with random yaw rotation
block_angle = random.random() * math.pi
block_orient = p.getQuaternionFromEuler([0, 0, block_angle])
obj_uid = p.loadURDF(urdf_filename, basePosition=block_pos, baseOrientation=block_orient, flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
self.obj_ids.append(obj_uid)
# let object fall and settle to be clear of next
for _ in range(500):
p.stepSimulation()
def destroy(objID):
"""destroys an object in both pybullet and unity
input:
objID: int
"""
p.removeBody(objID)
hu.unityDestroy(objID)
def build_kuka():
# Load Kuka robot.
robot = kuka.Kuka(urdfRootPath=pdata.getDataPath())
# Remove the tray object.
p.removeBody(robot.trayUid)
return robot
def reset(self):
self.ordered_joints = []
for ob in self.objects:
p.removeBody(ob)
if self.self_collision:
self.objects = p.loadMJCF(
os.path.join(pybullet_data.getDataPath(), "mjcf",
self.model_xml),
flags=p.URDF_USE_SELF_COLLISION +
p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
self.objects)
else:
self.objects = p.loadMJCF(
os.path.join(pybullet_data.getDataPath(), "mjcf",
self.model_xml))
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
self.objects)
self.robot_specific_reset()
s = self.calc_state(
) # optimization: calc_state() can calculate something in self.* for calc_potential() to use
return s
def remove_body(self, body_uid):
"""Remove the body.
Args:
body_uid: The body Unique ID.
"""
pybullet.removeBody(bodyUniqueId=body_uid, physicsClientId=self.uid)
def process_scene(path, clear=True):
walls, map_label, bodies = build_scene_full()
shots = 0
views = np.ndarray((SHOTS_PER_SCENE, VIEW_DIM), dtype=np.float32)
while shots < SHOTS_PER_SCENE:
rand = random_pos(walls)
coords = np.floor(rand / PIXEL_FRACTION).astype(np.int)
if map_label[coords[0], coords[1]] == 0:
continue
pos = np.array([
rand[0], rand[1],
PUPPER_HEIGHT - np.random.random() * HEIGHT_VARIATION
])
pitch = (1 - 2 * np.random.random()) * PITCH_VARIATION
roll = (1 - 2 * np.random.random()) * ROLL_VARIATION
rot = np.array([np.random.random() * 2 * math.pi, pitch, roll])
view = pack_viewpoint(pos, rot)
w, h, rgb, depth, seg = get_image(view)
im = Image.fromarray(rgb)
im.save(path + '/obs{}.png'.format(shots))
views[shots, :] = view
shots += 1
if clear:
for obj in bodies:
p.removeBody(obj)
bodies = []
np.savez(path + '/labels.npz', map_label=map_label, views=views)
return bodies
def create_pile(self, obj_info):
box_ids = self.create_temp_box(0.30, 1)
for path, mod_orn, mod_stiffness in obj_info:
margin = 0.025
r_x = random.uniform(self.obj_init_pos[0] - margin,
self.obj_init_pos[0] + margin)
r_y = random.uniform(self.obj_init_pos[1] - margin,
self.obj_init_pos[1] + margin)
yaw = random.uniform(0, np.pi)
pos = [r_x, r_y, 1.0]
obj_id, _, _ = self.load_obj(path, pos, yaw, mod_orn,
mod_stiffness)
for _ in range(10):
self.step_simulation()
self.wait_until_still(obj_id, 150)
self.wait_until_all_still()
for handle in box_ids:
p.removeBody(handle)
box_ids = self.create_temp_box(0.45, 2)
self.wait_until_all_still()
for handle in box_ids:
p.removeBody(handle)
self.wait_until_all_still()
self.update_obj_states()
def reset(self, env):
self.total_rewards = 0
self.object_points = {}
self.t = 0
self.task_stage = 1
self.cable_bead_ids = []
self.def_ids = []
# Add square target zone only to get the bag a certain color.
self.add_zone(env)
# Pose of the bag, sample mid-air to let it drop naturally.
bpos, _ = self.random_pose(env, self._bag_size)
self.base_pos = [bpos[0], bpos[1], self._drop_height]
self.base_orn = self._sample_bag_orientation()
# Add the bag, load info about top ring, and make a cable.
self.bag_id = self.add_bag(env, self.base_pos, self.base_orn)
self.understand_bag_top_ring(env, self.base_pos)
self.add_cable_ring(env)
# Env must begin before we can apply forces to perturb the bag.
env.start()
self._apply_small_force(num_iters=self.num_force_iters)
# Remove the zone ID -- only had this to make the bag color the same.
p.removeBody(self.zone_id)
time.sleep(self._settle_secs)
env.pause()
def respawn_car(self):
"""
Function to respawn the car from the arena.
Arguments:
None
Return Values:
None
"""
if self.husky is not None:
print("Old Car being Removed")
p.removeBody(self.husky)
self.husky = None
pos = [[5, 5]]
ori = [np.pi/2, 0, np.pi/2, np.pi]
x = np.random.randint(0, len(pos))
self.husky = p.loadURDF('rsc/car/car.urdf', [2.5-1*pos[x][0],2.5-1*pos[x][1],0], p.getQuaternionFromEuler([0,0,ori[x]]))
#self.husky = p.loadURDF('husky/husky.urdf', [4-1*pos[x][0],4-1*pos[x][1],0], p.getQuaternionFromEuler([0,0,ori[x]]))
#self.aruco = p.loadURDF('rsc/aruco/aruco.urdf', [4-1*pos[x][0],4-1*pos[x][1],1.2], p.getQuaternionFromEuler([1.5707,0,ori[x]]))
#p.createConstraint(self.husky, -1, self.aruco, -1, p.JOINT_FIXED, [0,0,1], [0,0,0.4], [0,0,0], childFrameOrientation = p.getQuaternionFromEuler([0,0,1]))
for x in range(100):
p.stepSimulation()
def remove_obj(self, obj_id):
# Get index of obj in id list, then remove object from simulation
idx = self.obj_ids.index(obj_id)
self.obj_orientations.pop(idx)
self.obj_positions.pop(idx)
self.obj_ids.pop(idx)
p.removeBody(obj_id)
def _flag_reposition(self):
self.walk_target_x = self.np_random.uniform(
low=-self.scene.stadium_halflen, high=+self.scene.stadium_halflen)
self.walk_target_y = self.np_random.uniform(
low=-self.scene.stadium_halfwidth,
high=+self.scene.stadium_halfwidth)
more_compact = 0.5 # set to 1.0 whole football field
self.walk_target_x *= more_compact / self.robot.mjcf_scaling
self.walk_target_y *= more_compact / self.robot.mjcf_scaling
self.flag = None
#self.flag = self.scene.cpp_world.debug_sphere(self.walk_target_x, self.walk_target_y, 0.2, 0.2, 0xFF8080)
self.flag_timeout = 3000 / self.scene.frame_skip
#print('targetxy', self.flagid, self.walk_target_x, self.walk_target_y, p.getBasePositionAndOrientation(self.flagid))
#p.resetBasePositionAndOrientation(self.flagid, posObj = [self.walk_target_x, self.walk_target_y, 0.5], ornObj = [0,0,0,0])
if self.gui:
if self.lastid:
p.removeBody(self.lastid)
self.lastid = p.createMultiBody(
baseVisualShapeIndex=self.visualid,
baseCollisionShapeIndex=-1,
basePosition=[self.walk_target_x, self.walk_target_y, 0.5])
self.robot.walk_target_x = self.walk_target_x
self.robot.walk_target_y = self.walk_target_y
def drawSolutionPath(start_pos, goal_pos, nn):
# To get the solution path out and show the solution in animation
global goal
global pose
global boxId
#Get the solution and show on tree
solution = []
solution.append(goal_pos)
while nn != start_pos:
solution.insert(0, nn)
plt.plot(nn.x, nn.y, 'ro')
plt.draw()
nn = nn.parent
solution.insert(0, start_pos)
#Show solution
#connect to GUI or DIRECT
PBT.connect(PBT.GUI)
#Set gravity
PBT.setGravity(0, 0, -10)
#Import ground plane
PBT.loadURDF("plane.urdf", [12, 12, 0])
#Import obstacle boxes
obsboxStartPos = [5, 2, 0]
obsboxStartOrientation = PBT.getQuaternionFromEuler([0, 0, 0])
PBT.loadURDF("box/urdf/box.urdf", obsboxStartPos, obsboxStartOrientation)
obsboxStartPos = [5, 20, 0]
obsboxStartOrientation = PBT.getQuaternionFromEuler([0, 0, 0])
PBT.loadURDF("box/urdf/box.urdf", obsboxStartPos, obsboxStartOrientation)
#Import HMMR at desired pos
cubeStartPos = [start_pos.x, start_pos.y, 0.14]
cubeStartOrientation = PBT.getQuaternionFromEuler(
[0.0, 0.0, start_pos.theta])
boxId = PBT.loadURDF("HMMR/urdf/HMMR.urdf", cubeStartPos,
cubeStartOrientation)
pose = [
start_pos.x, start_pos.y, start_pos.theta, start_pos.theta1,
start_pos.theta2
]
input("Set zoom press enter")
print("Solution: ")
for i in range(len(solution)):
print(solution[i].x, solution[i].y, solution[i].theta,
solution[i].theta1, solution[i].theta2)
goal = [
solution[i].x, solution[i].y, solution[i].theta,
solution[i].theta1, solution[i].theta2
]
while not at_goal():
gtg()
pose_update()
time.sleep(0.1)
pub_vel(0.0, 0.0)
PBT.removeBody(boxId)
PBT.disconnect()
def attach_object(self, object, parent_link_name, transform):
"""
Rigidly attach another object to the robot.
:param object: Object that shall be attached to the robot.
:type object: UrdfObject
:param parent_link_name: Name of the link to which the object shall be attached.
:type parent_link_name: str
:param transform: Hom. transform between the reference frames of the parent link and the object.
:type Transform
"""
# TODO should only be called through world because this class does not know which objects exist
if self.has_attached_object(object.name):
# TODO: choose better exception type
raise DuplicateNameException(
u'An object \'{}\' has already been attached to the robot.'.
format(object.name))
# salvage last joint state and base pose
joint_state = self.get_joint_states()
base_pose = self.get_base_pose()
# salvage last collision matrix, and save collisions as pairs of link names
collision_matrix = set()
for collision in self.sometimes:
collision_matrix.add((self.link_id_to_name[collision[0]],
self.link_id_to_name[collision[1]]))
# assemble and store URDF string of new link and fixed joint
new_joint = FixedJoint(u'{}_joint'.format(object.name), transform,
parent_link_name, object.name)
self.attached_objects[object.name] = u'{}{}'.format(
to_urdf_string(new_joint), to_urdf_string(object, True))
new_urdf_string = self.get_urdf()
# remove last robot and load new robot from new URDF
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.removeBody(self.id)
self.id = load_urdf_string_into_bullet(new_urdf_string, base_pose)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
# reload joint info and last joint state
self.init_js_info()
self.set_joint_state(joint_state)
# reload last collision matrix as pairs of link IDs
self.sometimes = set()
for collision in collision_matrix:
self.sometimes.add((self.link_name_to_id[collision[0]],
self.link_name_to_id[collision[1]]))
# update the collision matrix for the newly attached object
object_id = self.link_name_to_id[object.name]
link_pairs = {(object_id, link_id)
for link_id in self.joint_id_to_info.keys()}
new_collisions = self.calc_self_collision_matrix(link_pairs)
self.sometimes.union(new_collisions)
print(u'object {} attached to {} in pybullet world'.format(
object.name, self.name))
def __del__(self):
"""
Removes the block from the environment
"""
# At this point it may be that pybullet was already shut down. To avoid
# an error, only remove the object if the simulation is still running.
if pybullet.isConnected(**self._kwargs):
pybullet.removeBody(self.block, **self._kwargs)
def __del__(self):
"""
Removes the visual object from the environment
"""
# At this point it may be that pybullet was already shut down. To avoid
# an error, only remove the object if the simulation is still running.
if p.isConnected():
p.removeBody(self.body_id)
def reset(self): self.ordered_joints = [] for ob in self.objects: p.removeBody(ob) if self.self_collision: self.objects = p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml), flags=p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self.objects ) else: self.objects = p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml)) self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self.objects) self.robot_specific_reset() s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use return s
