def get_current_variable_values_for_arena(self):
"""
:return: (dict) returns all the exposed variables and their values
in the environment's stage except for objects.
"""
if self._pybullet_client_w_o_goal_id is not None:
client = self._pybullet_client_w_o_goal_id is not None
else:
client = self._pybullet_client_full_id
variable_params = dict()
variable_params["floor_color"] = \
pybullet.getVisualShapeData(WorldConstants.FLOOR_ID,
physicsClientId=client)[0][7][:3]
variable_params["floor_friction"] = \
pybullet.getDynamicsInfo(WorldConstants.FLOOR_ID, -1,
physicsClientId=client)[1]
variable_params["stage_color"] = \
pybullet.getVisualShapeData(WorldConstants.STAGE_ID,
physicsClientId=client)[0][7][:3]
variable_params["stage_friction"] = \
pybullet.getDynamicsInfo(WorldConstants.STAGE_ID, -1,
physicsClientId=client)[1]
variable_params["gravity"] = \
self._current_gravity
return variable_params
def _get_obs(self, skip_fixed: bool = False, skip_rigid: bool = False):
for oid in self.obj_ids['fixed']:
visual_data = p.getVisualShapeData(objectUniqueId=oid)
rgba = visual_data[0][-1]
if skip_fixed:
rgba = (rgba[0], rgba[1], rgba[2], 0)
else:
rgba = (rgba[0], rgba[1], rgba[2], 1)
p.changeVisualShape(objectUniqueId=oid,
linkIndex=-1,
rgbaColor=rgba)
for oid in self.obj_ids['rigid']:
visual_data = p.getVisualShapeData(objectUniqueId=oid)
rgba = visual_data[0][-1]
if skip_rigid:
rgba = (rgba[0], rgba[1], rgba[2], 0)
else:
rgba = (rgba[0], rgba[1], rgba[2], 1)
p.changeVisualShape(objectUniqueId=oid,
linkIndex=-1,
rgbaColor=rgba)
# Get RGB-D camera image observations.
obs = {'color': (), 'depth': ()}
for config in self.agent_cams:
color, depth, _ = self.render_camera(config)
obs['color'] += (color, )
obs['depth'] += (depth, )
return obs
def update_state(self):
image = None
if not self.stereo_images:
image = self.grab_image(
self.view_matrix, self.proj_matrix
) #[self.grab_image(self.view_matrix, self.proj_matrix)]
else:
image_l = self.grab_image(self.view_matrix_left,
self.proj_matrix_left)
image_r = self.grab_image(self.view_matrix_right,
self.proj_matrix_right)
image = [image_l, image_r]
item = Item([], [], [])
goal = Item([], [], [])
item_pose = p.getBasePositionAndOrientation(self.itemId)
item_dim = p.getVisualShapeData(self.itemId)[0][3]
goal_pose = p.getBasePositionAndOrientation(self.goalId)
goal_dim = p.getVisualShapeData(self.goalId)[0][3]
item.set_pos_and_ori_from_pose(item_pose)
item.set_dim(item_dim)
goal.set_pos_and_ori_from_pose(goal_pose)
goal.set_dim(goal_dim)
self.state = State(image, item, goal)
def info(self):
"""Normally returns dict of:
object id : (position, rotation, dimensions)
However, I made a few changes. First, removing IDs in some tasks, so
we shouldn't query their position. Second, adding object mesh for
gt_state and soft bodies. The second `if` test is redundant but just
in case. Here, we instead map to the mesh data instead of (position,
rotation, dimension). This is: (nb_vertices, (positions)) where the
latter is a tuple that has all the 3D positions of each vertex in the
simulation mesh, e.g., it's 100 for cloth.
Note on soft body IDs:
cloth-cover: ID 5 is cloth (ID 4 is item to cover)
cloth-flat(notarget): ID 5 is cloth (ID 4 is the zone, though in the no
target case we remove it ... historical reasons).
bag tasks: all have ID 5 as the bag, because they use ID 4 for the zone,
(with bag-alone removing zone) and other items are added afterwards.
To see how we use the special case for soft bodies, see:
ravens/agents/gt_state.py and the extraact_x_y_theta method.
We depend on assuming that len(info[id]) = 2 instead of 3.
"""
removed_IDs = []
if (isinstance(self.task, tasks.names['cloth-flat-notarget']) or
isinstance(self.task, tasks.names['bag-alone-open'])):
removed_IDs.append(self.task.zone_ID)
# Daniel: special case for soft bodies (and gt_state). For now only cloth.
softbody_id = -1
if self.is_cloth_env():
assert len(self.task.def_IDs) == 1, self.task.def_IDs
softbody_id = self.task.def_IDs[0]
# object id : (position, rotation, dimensions)
info = {}
for object_id in (self.fixed_objects + self.objects):
if object_id in removed_IDs:
continue
# Daniel: special cases for soft bodies (and gt_state), and then bags.
if (object_id == softbody_id) and self.is_cloth_env():
info[object_id] = p.getMeshData(object_id, -1, flags=p.MESH_DATA_SIMULATION_MESH)
elif isinstance(self.task, tasks.names['bag-color-goal']):
# Note: we don't do this for sorting? :X
position, rotation = p.getBasePositionAndOrientation(object_id)
dimensions = p.getVisualShapeData(object_id)[0][3]
rgba_color = p.getVisualShapeData(object_id)[0][7] # see pybullet docs
assert rgba_color[3] == 1, rgba_color
rgb_color = rgba_color[0:3] # we can ignore the last component it is always 1
info[object_id] = (position, rotation, dimensions, rgb_color)
else:
# The usual case.
position, rotation = p.getBasePositionAndOrientation(object_id)
dimensions = p.getVisualShapeData(object_id)[0][3]
info[object_id] = (position, rotation, dimensions)
return info
def test_color(self):
black_color = (0, 0, 0, 1)
white_color = (1, 1, 1, 1)
RobotSphereClassTest.robot.setColor(black_color)
shape_data =\
p.getVisualShapeData(RobotSphereClassTest.robot.getRobotModel(),
physicsClientId=RobotSphereClassTest.client)
self.assertTrue(black_color == shape_data[0][-1])
RobotSphereClassTest.robot.setColor(white_color)
shape_data =\
p.getVisualShapeData(RobotSphereClassTest.robot.getRobotModel(),
physicsClientId=RobotSphereClassTest.client)
self.assertTrue(white_color == shape_data[0][-1])
def reset(self):
p.resetSimulation()
self.step_counter = 0
#载入yumi
self.yumiUid = p.loadURDF(
os.path.join(os.path.dirname(os.path.realpath(__file__)),
"assets/yumi_with_hands.urdf"),
useFixedBase=True,
flags=p.URDF_USE_SELF_COLLISION +
p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
#载入地面
self.planeUid = p.loadURDF(os.path.join(pybullet_data.getDataPath(),
"plane.urdf"),
basePosition=[0, 0, -0.65])
#载入桌子
self.tableUid = p.loadURDF(os.path.join(pybullet_data.getDataPath(),
"table/table.urdf"),
basePosition=[0.5, 0, -0.65])
p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(1. / 240.)
self.joint2Index = {} # jointIndex map to jointName
for i in range(p.getNumJoints(self.yumiUid)):
self.joint2Index[p.getJointInfo(self.yumiUid,
i)[1].decode('utf-8')] = i
self.jointColor = {} # jointName map to jointColor
for data in p.getVisualShapeData(self.yumiUid):
self.jointColor[p.getJointInfo(
self.yumiUid, data[1])[1].decode('utf-8')] = data[7]
# recover color
for joint, index in self.joint2Index.items():
if joint in self.jointColor and joint != 'world_joint':
p.changeVisualShape(self.yumiUid,
index,
rgbaColor=self.jointColor[joint])
def reset(self, env):
self.total_rewards = 0
# Add goal post.
line_urdf = 'assets/line/line.urdf'
self.zone_size = (0.5, 1, 0.2)
self.zone_pose = ((0.5, -0.86, 0), p.getQuaternionFromEuler((0, 0, 0)))
env.add_object(line_urdf, self.zone_pose, fixed=True)
# Add box.
box_size = self.random_size(0.05, 0.25, 0.05, 0.15, 0.05, 0.05)
box_template = 'assets/box/box-template.urdf'
box_urdf = self.fill_template(box_template, {'DIM': box_size})
px = self.bounds[0, 0] + 0.1 + np.random.rand() * 0.3
position = (px, 0.3, box_size[2] / 2)
theta = np.random.rand() * np.pi / 4 - np.pi / 8
rotation = p.getQuaternionFromEuler((0, 0, theta))
box_pose = (position, rotation)
box_id = env.add_object(box_urdf, box_pose)
os.remove(box_urdf)
self.color_random_brown(box_id)
self.object_points = {box_id: self.get_object_points(box_id)}
# Move end effector to start position next to box.
box_dim = p.getVisualShapeData(box_id)[0][3]
start_position = np.array([0, box_dim[1] / 2 + 0.01, 0])
start_position = self.apply(box_pose, start_position)
rotation = tuple(env.home_pose[3:])
joints = env.solve_IK((tuple(start_position) + rotation))
for i in range(len(env.joints)):
p.resetJointState(env.ur5, env.joints[i], joints[i])
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink, physicsClientId):
dyn = p.getDynamicsInfo(bodyUid, linkIndex, physicsClientId=physicsClientId)
urdfLink.urdf_inertial.mass = dyn[0]
urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
# todo
urdfLink.urdf_inertial.origin_xyz = dyn[3]
urdfLink.urdf_inertial.origin_rpy = p.getEulerFromQuaternion(dyn[4])
visualShapes = p.getVisualShapeData(bodyUid, physicsClientId=physicsClientId)
matIndex = 0
for v in visualShapes:
if (v[1] == linkIndex):
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
if (v[2] == p.GEOM_BOX):
urdfVisual.geom_extents = v[3]
if (v[2] == p.GEOM_SPHERE):
urdfVisual.geom_radius = v[3][0]
if (v[2] == p.GEOM_MESH):
urdfVisual.geom_meshfilename = v[4].decode("utf-8")
if urdfVisual.geom_meshfilename == UNKNOWN_FILE:
continue
urdfVisual.geom_meshscale = v[3]
if (v[2] == p.GEOM_CYLINDER):
urdfVisual.geom_length = v[3][0]
urdfVisual.geom_radius = v[3][1]
if (v[2] == p.GEOM_CAPSULE):
urdfVisual.geom_length = v[3][0]
urdfVisual.geom_radius = v[3][1]
urdfVisual.origin_xyz = v[5]
urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
urdfVisual.material_rgba = v[7]
name = 'mat_{}_{}'.format(linkIndex, matIndex)
urdfVisual.material_name = name
urdfLink.urdf_visual_shapes.append(urdfVisual)
matIndex = matIndex + 1
collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex, physicsClientId=physicsClientId)
for v in collisionShapes:
urdfCollision = UrdfCollision()
urdfCollision.geom_type = v[2]
if (v[2] == p.GEOM_BOX):
urdfCollision.geom_extents = v[3]
if (v[2] == p.GEOM_SPHERE):
urdfCollision.geom_radius = v[3][0]
if (v[2] == p.GEOM_MESH):
urdfCollision.geom_meshfilename = v[4].decode("utf-8")
if urdfCollision.geom_meshfilename == UNKNOWN_FILE:
continue
urdfCollision.geom_meshscale = v[3]
if (v[2] == p.GEOM_CYLINDER):
urdfCollision.geom_length = v[3][0]
urdfCollision.geom_radius = v[3][1]
if (v[2] == p.GEOM_CAPSULE):
urdfCollision.geom_length = v[3][0]
urdfCollision.geom_radius = v[3][1]
pos, orn = p.multiplyTransforms(dyn[3], dyn[4], v[5], v[6])
urdfCollision.origin_xyz = pos
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
urdfLink.urdf_collision_shapes.append(urdfCollision)
def get_current_variable_values(self):
"""
:return: (dict) returns all the exposed variables in the environment along with their
corresponding values.
"""
# TODO: not a complete list yet of what we want to expose
variable_params = dict()
variable_params['joint_positions'] = self._latest_full_state[
'positions']
variable_params['control_index'] = self._control_index
variable_params['joint_velocities'] = self._latest_full_state[
'velocities']
if self._pybullet_client_w_o_goal_id is not None:
client = self._pybullet_client_w_o_goal_id
else:
client = self._pybullet_client_full_id
position, _ = pybullet. \
getBasePositionAndOrientation(WorldConstants.ROBOT_ID,
physicsClientId=
client)
variable_params[
'robot_height'] = position[-1] + WorldConstants.ROBOT_HEIGHT
for robot_finger_link in WorldConstants.LINK_IDS:
variable_params[robot_finger_link] = dict()
variable_params[robot_finger_link]['color'] = \
pybullet.getVisualShapeData(WorldConstants.ROBOT_ID,
physicsClientId=client)\
[WorldConstants.VISUAL_SHAPE_IDS[robot_finger_link]][7][:3]
variable_params[robot_finger_link]['mass'] = \
pybullet.getDynamicsInfo(WorldConstants.ROBOT_ID,
WorldConstants.LINK_IDS[robot_finger_link],
physicsClientId=client)[0]
return variable_params
def test_texture(self):
p.setAdditionalSearchPath(
os.path.dirname(os.path.realpath(__file__)),
physicsClientId=RobotSphereClassTest.client)
texture_path_name = "This is not a texture"
try:
RobotSphereClassTest.robot.setTexture(texture_path_name)
except Exception as error:
print("Normal error", error)
self.assertTrue(
RobotSphereClassTest.robot.getTextureId() is None)
self.assertTrue(
RobotSphereClassTest.robot.getTexturePathName() !=\
texture_path_name)
texture_path_name = "data/texture.png"
RobotSphereClassTest.robot.setTexture(texture_path_name)
time.sleep(1)
shape_data =\
p.getVisualShapeData(RobotSphereClassTest.robot.getRobotModel(),
flags=p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS,
physicsClientId=RobotSphereClassTest.client)
# Texture id is not updating in pybullet code, to check
# self.assertTrue(shape_data[0][-1] != -1)
self.assertTrue(
RobotSphereClassTest.robot.getTexturePathName() ==\
texture_path_name)
def import_scene(self,
scene,
texture_scale=1.0,
load_texture=True,
class_id=None):
"""
Import a scene into the simulator. A scene could be a synthetic one or a realistic Gibson Environment.
:param scene: Scene object
:param texture_scale: Option to scale down the texture for rendering
:param load_texture: If you don't need rgb output, texture loading could be skipped to make rendering faster
:param class_id: Class id for rendering semantic segmentation
"""
if class_id is None:
class_id = self.next_class_id
self.next_class_id += 1
new_objects = scene.load()
for item in new_objects:
self.objects.append(item)
for new_object in new_objects:
for shape in p.getVisualShapeData(new_object):
id, link_id, type, dimensions, filename, rel_pos, rel_orn, color = shape[:
8]
visual_object = None
if type == p.GEOM_MESH:
filename = filename.decode('utf-8')
if filename not in self.visual_objects.keys():
self.renderer.load_object(filename,
texture_scale=texture_scale,
load_texture=load_texture)
self.visual_objects[filename] = len(
self.renderer.visual_objects) - 1
visual_object = self.visual_objects[filename]
elif type == p.GEOM_PLANE:
pass
# By default, we add an additional floor surface to "smooth out" that of the original mesh.
# Normally you don't need to render this additionally added floor surface.
# However, if you do want to render it for some reason, you can uncomment the block below.
# filename = os.path.join(gibson2.assets_path, 'models/mjcf_primitives/cube.obj')
# self.renderer.load_object(filename,
# transform_orn=rel_orn,
# transform_pos=rel_pos,
# input_kd=color[:3],
# scale=[100, 100, 0.01])
# visual_object = len(self.renderer.visual_objects) - 1
if visual_object is not None:
self.renderer.add_instance(visual_object,
pybullet_uuid=new_object,
class_id=class_id)
if scene.is_interactive:
for obj in scene.scene_objects:
self.import_articulated_object(obj)
self.scene = scene
return new_objects
def _reset(self):
self.terminated = False
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
self._envStepCounter = 0
# 加载地面
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"),
useFixedBase=True)
# Load robot
self._kuka = kukakr6(self._urdfRoot,
timeStep=self._timeStep,
basePosition=[0, 0, 0.75],
useInverseKinematics=self._useIK,
action_space=self.action_dim,
includeVelObs=self.includeVelObs,
init_joints=self.init_joints)
# 桌子
tableId = p.loadURDF(os.path.join(self._urdfRoot,
"kuka_kr6_support/table.urdf"),
useFixedBase=True)
# tableId = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "table.urdf"), useFixedBase=True)
table_info = p.getVisualShapeData(tableId, -1)[0]
self._h_table = table_info[5][2] + table_info[3][2]
# limit panda workspace to table plane
self._kuka.workspace_lim[2][0] = self._h_table
p.setGravity(0, 0, -9.8)
def _get_data(self, index):
if self._data is None:
body_data = pb.getVisualShapeData(self._body_id,
physicsClientId=self.client_id)
link_data = next(
(i for i in body_data if i[1] == self._link_index))
self._data = link_data
return self._data[index]
def bullet2pyqtgraph(Id):
shape_data = p.getVisualShapeData(Id)
mesh_items = []
for i in range(len(shape_data)):
# process item i
mesh = visualGeometryType2mesh(shape_data[i])
mesh_items.append(mesh)
return mesh_items
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink, physicsClientId):
dyn = p.getDynamicsInfo(bodyUid,linkIndex,physicsClientId=physicsClientId)
urdfLink.urdf_inertial.mass = dyn[0]
urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
#todo
urdfLink.urdf_inertial.origin_xyz = dyn[3]
rpy = p.getEulerFromQuaternion(dyn[4])
urdfLink.urdf_inertial.origin_rpy = rpy
visualShapes = p.getVisualShapeData(bodyUid,physicsClientId=physicsClientId)
matIndex = 0
for v in visualShapes:
if (v[1]==linkIndex):
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
if (v[2]==p.GEOM_BOX):
urdfVisual.geom_extents = v[3]
if (v[2]==p.GEOM_SPHERE):
urdfVisual.geom_radius = v[3][0]
if (v[2]==p.GEOM_MESH):
urdfVisual.geom_meshfilename = v[4].decode("utf-8")
urdfVisual.geom_meshscale = v[3]
if (v[2]==p.GEOM_CYLINDER):
urdfVisual.geom_length=v[3][0]
urdfVisual.geom_radius=v[3][1]
if (v[2]==p.GEOM_CAPSULE):
urdfVisual.geom_length=v[3][0]
urdfVisual.geom_radius=v[3][1]
urdfVisual.origin_xyz = v[5]
urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
urdfVisual.material_rgba = v[7]
name = 'mat_{}_{}'.format(linkIndex,matIndex)
urdfVisual.material_name = name
urdfLink.urdf_visual_shapes.append(urdfVisual)
matIndex=matIndex+1
collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex,physicsClientId=physicsClientId)
for v in collisionShapes:
urdfCollision = UrdfCollision()
urdfCollision.geom_type = v[2]
if (v[2]==p.GEOM_BOX):
urdfCollision.geom_extents = v[3]
if (v[2]==p.GEOM_SPHERE):
urdfCollision.geom_radius = v[3][0]
if (v[2]==p.GEOM_MESH):
urdfCollision.geom_meshfilename = v[4].decode("utf-8")
urdfCollision.geom_meshscale = v[3]
if (v[2]==p.GEOM_CYLINDER):
urdfCollision.geom_length=v[3][0]
urdfCollision.geom_radius=v[3][1]
if (v[2]==p.GEOM_CAPSULE):
urdfCollision.geom_length=v[3][0]
urdfCollision.geom_radius=v[3][1]
pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
v[5], v[6])
urdfCollision.origin_xyz = pos
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
urdfLink.urdf_collision_shapes.append(urdfCollision)
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink):
dyn = p.getDynamicsInfo(bodyUid, linkIndex)
urdfLink.urdf_inertial.mass = dyn[0]
urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
#todo
urdfLink.urdf_inertial.origin_xyz = dyn[3]
rpy = p.getEulerFromQuaternion(dyn[4])
urdfLink.urdf_inertial.origin_rpy = rpy
visualShapes = p.getVisualShapeData(bodyUid)
matIndex = 0
for v in visualShapes:
if (v[1] == linkIndex):
print("visualShape base:", v)
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
if (v[2] == p.GEOM_BOX):
urdfVisual.geom_extents = v[3]
if (v[2] == p.GEOM_SPHERE):
urdfVisual.geom_radius = v[3][0]
if (v[2] == p.GEOM_MESH):
urdfVisual.geom_meshfile = v[4].decode("utf-8")
if (v[2] == p.GEOM_CYLINDER):
urdfVisual.geom_radius = v[3][1]
urdfVisual.geom_length = v[3][0]
urdfVisual.origin_xyz = v[5]
urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
urdfVisual.material_rgba = v[7]
name = 'mat_{}_{}'.format(linkIndex, matIndex)
urdfVisual.material_name = name
urdfLink.urdf_visual_shapes.append(urdfVisual)
matIndex = matIndex + 1
collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex)
for v in collisionShapes:
print("collisionShape base:", v)
urdfCollision = UrdfCollision()
print("geom type=", v[0])
urdfCollision.geom_type = v[2]
if (v[2] == p.GEOM_BOX):
urdfCollision.geom_extents = v[3]
if (v[2] == p.GEOM_SPHERE):
urdfCollision.geom_radius = v[3][0]
if (v[2] == p.GEOM_MESH):
urdfCollision.geom_meshfile = v[4].decode("utf-8")
#localInertiaFrame*childTrans
if (v[2] == p.GEOM_CYLINDER):
urdfCollision.geom_radius = v[3][1]
urdfCollision.geom_length = v[3][0]
pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
v[5], v[6])
urdfCollision.origin_xyz = pos
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
urdfLink.urdf_collision_shapes.append(urdfCollision)
def get_object_points(self, obj):
obj_shape = p.getVisualShapeData(obj)
obj_dim = obj_shape[0][3]
xv, yv, zv = np.meshgrid(
np.arange(-obj_dim[0] / 2, obj_dim[0] / 2, 0.02),
np.arange(-obj_dim[1] / 2, obj_dim[1] / 2, 0.02),
np.arange(-obj_dim[2] / 2, obj_dim[2] / 2, 0.02),
sparse=False, indexing='xy')
return np.vstack((xv.reshape(1, -1), yv.reshape(1, -1), zv.reshape(1, -1)))
def _setup_gripper(self):
# add kuka arm
# self.armId = p.loadSDF("kuka_iiwa/kuka_with_gripper2.sdf")[0]
# self.armId = p.loadURDF("franka_panda/panda.urdf", useFixedBase=True)
self.armId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0],
useFixedBase=True)
p.resetBasePositionAndOrientation(self.armId, [0, 0, 0], [0, 0, 0, 1])
# TODO modify dynamics to induce traps
# for j in range(p.getNumJoints(self.armId)):
# p.changeDynamics(self.armId, j, linearDamping=0, angularDamping=0)
# orientation of the end effector
self.endEffectorOrientation = p.getQuaternionFromEuler(
[0, math.pi / 2, 0])
self.endEffectorIndex = kukaEndEffectorIndex
self.numJoints = p.getNumJoints(self.armId)
# get the joint ids
# TODO try out arm with attached grippers
# self.armInds = [i for i in range(pandaNumDofs)]
self.armInds = [i for i in range(self.numJoints)]
# create a constraint to keep the fingers centered
# c = p.createConstraint(self.armId,
# 9,
# self.armId,
# 10,
# jointType=p.JOINT_GEAR,
# jointAxis=[1, 0, 0],
# parentFramePosition=[0, 0, 0],
# childFramePosition=[0, 0, 0])
# p.changeConstraint(c, gearRatio=-1, erp=0.1, maxForce=50)
# joint damping coefficents
# self.jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
# self.jd = [
# 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
# 0.00001, 0.00001, 0.00001, 0.00001
# ]
p.enableJointForceTorqueSensor(self.armId, self.endEffectorIndex)
self._calculate_init_joints()
for i in self.armInds:
p.resetJointState(self.armId, i, self.initJoints[i])
# self._open_gripper()
# self._close_gripper()
# make arm translucent
visual_data = p.getVisualShapeData(self.armId)
for link in visual_data:
link_id = link[1]
if link_id == -1:
continue
rgba = list(link[7])
rgba[3] = 0.4
p.changeVisualShape(self.armId, link_id, rgbaColor=rgba)
def import_scene(self,
scene,
texture_scale=1.0,
load_texture=True,
class_id=0):
"""
Import a scene. A scene could be a synthetic one or a realistic Gibson Environment.
:param scene: Scene object
:param texture_scale: Option to scale down the texture for rendering
:param load_texture: If you don't need rgb output, texture loading could be skipped to make rendering faster
:param class_id: The class_id for background for rendering semantics.
"""
new_objects = scene.load()
for item in new_objects:
self.objects.append(item)
for new_object in new_objects:
for shape in p.getVisualShapeData(new_object):
id, link_id, type, dimensions, filename, rel_pos, rel_orn, color = shape[:
8]
if type == p.GEOM_MESH:
filename = filename.decode('utf-8')
if not filename in self.visual_objects.keys():
self.renderer.load_object(filename,
texture_scale=texture_scale,
load_texture=load_texture)
self.visual_objects[filename] = len(
self.renderer.visual_objects) - 1
self.renderer.add_instance(
len(self.renderer.visual_objects) - 1,
pybullet_uuid=new_object,
class_id=class_id)
else:
self.renderer.add_instance(
self.visual_objects[filename],
pybullet_uuid=new_object,
class_id=class_id)
elif type == p.GEOM_PLANE:
pass #don't load plane, it will cause z fighting
#filename = os.path.join(gibson2.assets_path, 'models/mjcf_primitives/cube.obj')
# self.renderer.load_object(filename,
# transform_orn=rel_orn,
# transform_pos=rel_pos,
# input_kd=color[:3],
# scale=[100, 100, 0.01])
# self.renderer.add_instance(len(self.renderer.visual_objects) - 1,
# pybullet_uuid=new_object,
# class_id=class_id,
# dynamic=True)
self.scene = scene
return new_objects
def reset(self):
self.terminated = False
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
self._envStepCounter = 0
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"),
useFixedBase=True)
# Load robot
self._panda = pandaEnv(self._urdfRoot,
timeStep=self._timeStep,
basePosition=[0, 0, 0.625],
useInverseKinematics=self._useIK,
action_space=self.action_dim,
includeVelObs=self.includeVelObs)
# Load table and object for simulation
tableId = p.loadURDF(os.path.join(self._urdfRoot,
"franka_description/table.urdf"),
useFixedBase=True)
table_info = p.getVisualShapeData(tableId, -1)[0]
self._h_table = table_info[5][2] + table_info[3][2]
#limit panda workspace to table plane
self._panda.workspace_lim[2][0] = self._h_table
# Randomize start position of object and target.
#we take the target point
self.obj_pose, self.target_pose = self._sample_pose()
if (self.fixedPositionObj):
#we use a fixed starting position for the cube
self._objID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/cube_small.urdf"),
basePosition=[0.7, 0.0, 0.64])
else:
self._objID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/cube_small.urdf"),
basePosition=self.obj_pose)
#useful to see where is the taget point
self._targetID = p.loadURDF(
os.path.join(self._urdfRoot,
"franka_description/domino/domino.urdf"),
self.target_pose)
self._debugGUI()
p.setGravity(0, 0, -9.8)
# Let the world run for a bit
for _ in range(10):
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def get_shape(self, body, flags=0):
"""
Get body's shape
:param body: int
body's ID
:param flags: int
Texture unique id
:return: tuple
Body shape information
"""
return p.getVisualShapeData(body, flags, physicsClientId=self.client_id)
def print_log(object_id, force):
shape_data = p.getVisualShapeData(object_id)[0]
dynamics_data = p.getDynamicsInfo(object_id, -1)
print('***********************************')
print('Object: %s' % shape_data[4])
print('Dimensions: (%f, %f, %f)' % shape_data[3])
print('Mass: %f' % dynamics_data[0])
print('Lateral Friction: %f' % dynamics_data[1])
print('Inertia Diagonal: (%f, %f, %f)' % dynamics_data[2])
print('Force: (%f, %f, %f)' % tuple(force))
print('***********************************')
def get_entity(self, id):
""" Fetch an entity in the simulator and perform a lazzy update of the corresponding scene node
"""
if id not in self.nodes_map:
raise ValueError("Invalid id provided : '{}'".format(id))
scene_node = self.nodes_map[id]
sim_id = self.entity_id_map[id]
visual_shapes = p.getVisualShapeData(sim_id)
for idx, shape in enumerate(visual_shapes):
primitive_shape = scene_node.shapes[idx]
link_id = shape[1]
position = shape[5]
orientation = shape[6]
if link_id != -1:
link_state = p.getLinkState(sim_id, link_id)
t_link = link_state[0]
q_link = link_state[1]
t_inertial = link_state[2]
q_inertial = link_state[3]
tf_world_link = np.dot(translation_matrix(t_link), quaternion_matrix(q_link))
tf_inertial_link = np.dot(translation_matrix(t_inertial), quaternion_matrix(q_inertial))
world_transform = np.dot(tf_world_link, np.linalg.inv(tf_inertial_link))
else:
t_link, q_link = p.getBasePositionAndOrientation(sim_id)
world_transform = np.dot(translation_matrix(t_link), quaternion_matrix(q_link))
if link_id != -1:
shape_transform = np.dot(translation_matrix(position), quaternion_matrix(orientation))
shape_transform = np.dot(world_transform, shape_transform)
shape_transform = np.linalg.inv(np.dot(np.linalg.inv(shape_transform), scene_node.pose.transform()))
position = translation_from_matrix(shape_transform)
orientation = quaternion_from_matrix(shape_transform)
primitive_shape.pose.pos.x = position[0]
primitive_shape.pose.pos.y = position[1]
primitive_shape.pose.pos.z = position[2]
primitive_shape.pose.from_quaternion(orientation[0], orientation[1], orientation[2], orientation[3])
else:
shape_transform = np.dot(translation_matrix(position), quaternion_matrix(orientation))
shape_transform = np.dot(world_transform, shape_transform)
position = translation_from_matrix(shape_transform)
orientation = quaternion_from_matrix(shape_transform)
scene_node.pose.pos.x = position[0]
scene_node.pose.pos.y = position[1]
scene_node.pose.pos.z = position[2]
scene_node.pose.from_quaternion(orientation[0], orientation[1], orientation[2], orientation[3])
return self.nodes_map[id]
def _get_object_state(self, id):
pos, orn = pb.getBasePositionAndOrientation(id)
size = pb.getVisualShapeData(id)[0][3]
orn = pb.getEulerFromQuaternion(orn)
lv, av = pb.getBaseVelocity(id)
return {
"position": np.array(pos),
"size": np.array(size),
"orientation": np.array(orn),
"lin_vel": np.array(lv),
"ang_vel": np.array(av)
}
def test_scale_dimension(self):
robot_pose_init = RobotSphereClassTest.robot.getPosition()
init_dimension = RobotSphereClassTest.robot.getDimension()
RobotSphereClassTest.robot.setScale(2)
self.assertTrue(RobotSphereClassTest.robot.getScale() == 2)
robot_pose_actual = RobotSphereClassTest.robot.getPosition()
self.assertTrue(robot_pose_actual == robot_pose_init)
shape_data =\
p.getVisualShapeData(RobotSphereClassTest.robot.getRobotModel(),
physicsClientId=RobotSphereClassTest.client)
actual_dimensions = shape_data[0][3]
self.assertTrue(
init_dimension*RobotSphereClassTest.robot.getScale() ==\
actual_dimensions[0])
def reset(self):
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
self._envStepCounter = 0
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"),
[0, 0, 0])
# Load robot
self._icub = iCubEnv(urdfRootPath=self._urdfRoot,
timeStep=self._timeStep,
useInverseKinematics=self._useIK,
arm=self._control_arm,
useOrientation=self._useOrientation)
## Load table and object for simulation
self._tableId = p.loadURDF(
os.path.join(pybullet_data.getDataPath(), "table/table.urdf"),
[0.85, 0.0, 0.0])
#limit iCub workspace to table plane
table_info = p.getVisualShapeData(self._tableId, -1)[0]
self._h_table = table_info[5][2] + table_info[3][2]
self._icub.workspace_lim[2][0] = self._h_table
# Randomize start position of object
obj_pose, self._tg_pose = self._sample_pose()
self._objID = p.loadURDF(
os.path.join(pybullet_data.getDataPath(), "lego/lego.urdf"),
obj_pose)
self._debugGUI()
p.setGravity(0, 0, -9.8)
# Let the world run for a bit
for _ in range(10):
p.stepSimulation()
self._observation = self.getExtendedObservation()
self._init_dist_hand_obj = goal_distance(
np.array(self._observation[0:3]), np.array(obj_pose))
self._max_dist_obj_tg = goal_distance(np.array(obj_pose),
np.array(self._tg_pose))
return np.array(self._observation)
def _compute_workspace(self, table_id):
table_shape = p.getVisualShapeData(table_id, -1)
table_top_shape = table_shape[0]
table_height = table_top_shape[5][2] + table_top_shape[3][2] / 2
table_leg_pos = table_shape[1][5]
# Rotation of the vector is needed
# getVisualShapeData returns position of local visual frame, relative to link/joint frame
table_leg_pos = p.rotateVector(self._table_orn, table_leg_pos)
max_workspace_x = abs(table_leg_pos[0])
max_workspace_y = abs(table_leg_pos[1])
return [
[-max_workspace_x + 0.3, max_workspace_x - 0.05], # X
[-max_workspace_y + 0.3, max_workspace_y - 0.3], # Y
[table_height, 1] # Z
]
def storeVisualShapes(self, multiBodyId, pos, orn):
visualShapes = p.getVisualShapeData(multiBodyId)
for visualShape in visualShapes:
linkIndex = visualShape[1]
visualType = visualShape[2]
meshPath = visualShape[4]
pos = TranslationMatrix(visualShape[5])
orn = getOrnMatrixFromQuaternion(visualShape[6])
if (visualType == p.GEOM_MESH):
meshPath = meshPath.decode("utf-8")
wavefrontData = pywavefront.Wavefront(meshPath)
wavefrontVisualizer = WavefrontVisualiser(
wavefrontData, pos, orn)
self.visualShapes[multiBodyId][linkIndex] = VisualShape(
linkIndex, wavefrontVisualizer, pos, orn)
def info(self):
"""Environment info variable with object poses, dimensions, and colors."""
# Some tasks create and remove zones, so ignore those IDs.
# removed_ids = []
# if (isinstance(self.task, tasks.names['cloth-flat-notarget']) or
# isinstance(self.task, tasks.names['bag-alone-open'])):
# removed_ids.append(self.task.zone_id)
info = {} # object id : (position, rotation, dimensions)
for obj_ids in self.obj_ids.values():
for obj_id in obj_ids:
pos, rot = p.getBasePositionAndOrientation(obj_id)
dim = p.getVisualShapeData(obj_id)[0][3]
info[obj_id] = (pos, rot, dim)
return info
def info(self):
"""Environment info dictionary."""
# Some tasks create and remove zones, so ignore those IDs.
removed_ids = []
if (isinstance(self.task, tasks.names['cloth-flat-notarget'])
or isinstance(self.task, tasks.names['bag-alone-open'])):
removed_ids.append(self.task.zone_id)
info = {} # object id : (position, rotation, dimensions)
for object_id in self.fixed_objects + self.objects:
if object_id in removed_ids:
continue
position, rotation = p.getBasePositionAndOrientation(object_id)
dimensions = p.getVisualShapeData(object_id)[0][3]
info[object_id] = (position, rotation, dimensions)
return info
def get_world_visual_pose(body_unique_id: int, link_index: int):
"""Pose of visual frame with respect to world frame expressed in world frame.
Args:
body_unique_id (int): The body unique id, as returned by loadURDF, etc.
link_index (int): Link index or -1 for the base.
Returns:
pos_orn (tuple): See description.
"""
world_link_pose = get_world_link_pose(body_unique_id, link_index)
visual_shape_data = p.getVisualShapeData(body_unique_id, link_index)
local_visual_pose = (visual_shape_data[link_index + 1][5],
visual_shape_data[link_index + 1][6])
return p.multiplyTransforms(world_link_pose[0], world_link_pose[1],
local_visual_pose[0], local_visual_pose[1])
import pybullet as p
p.connect(p.GUI)
plane = p.loadURDF("plane.urdf")
visualData = p.getVisualShapeData(plane, p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS)
print(visualData)
curTexUid = visualData[0][8]
print(curTexUid)
texUid = p.loadTexture("tex256.png")
print("texUid=", texUid)
p.changeVisualShape(plane, -1, textureUniqueId=texUid)
for i in range(100):
p.getCameraImage(320, 200)
p.changeVisualShape(plane, -1, textureUniqueId=curTexUid)
for i in range(100):
p.getCameraImage(320, 200)
