def __init__(self, body_id, link_index, client_id):
self._body_id = body_id
self._link_index = link_index
self.client_id = client_id
self._data = pb.getCollisionShapeData(body_id,
link_index,
physicsClientId=client_id)
def reset(self):
# pybullet data path -> /home_directory/.local/lib/python3.8/site-packages/pybullet_data
pybullet_data_path = pybullet_data.getDataPath()
#print("=> data path -> {0}".format(pybullet_data_path))
# Add a search data path
p.setAdditionalSearchPath(pybullet_data_path)
# pybullet data path -> /home_directory/.local/lib/python3.8/site-packages/pybullet_data
p.loadURDF("plane.urdf", [0, 0, 0],
physicsClientId=self._physics_client_id)
# Load table and object
self.table_id = p.loadURDF("table/table.urdf",
basePosition=[0.85, 0.0, 0.0],
useFixedBase=True,
physicsClientId=self._physics_client_id)
table_info = p.getCollisionShapeData(
self.table_id, -1, physicsClientId=self._physics_client_id)[0]
# p.getCollisionShapeData return a list of collision shape, in our case there is only one shape so [0]
# table_info[5] (x,y,z) -> position of the collision shape (box) into table frame
# table_info[3] (x,y,z) -> size of collision shape (box)
# voir doc/table_collision.pdf
self._h_table = table_info[5][2] + table_info[3][2] / 2
# set ws limit on z according to table height
#print('world limit before table = ', self._ws_lim)
self._ws_lim[2][:] = [self._h_table, self._h_table + 0.3]
#print('world limit = ', self._ws_lim)
self.load_object(self._obj_name)
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 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 generatePointCloud(uId) -> o3d.geometry.PointCloud:
'''
Generate Open3D pointcloud from pyBullet URDF ID.
The poincloud has its origin at the base of
'''
# 1. Collect all shape data of parts along links in URDF
# Link index description
# -1: base
# 0: link after first joint
#
# partsInfo = [ part1_info , part2_info , ... ]
# Check pybulletQuickStartGuide for the return type of p.getCollisionShapeData()
partsInfo = []
for linkId in range(-1, p.getNumJoints(uId)):
linkInfo = p.getCollisionShapeData(uId, linkId)
for part in linkInfo:
partsInfo.append(part)
# 2. Calcuated total volume of the URDF. The volume is later used to decide the number of points cloud.
TotalVolume = 0
for i in partsInfo:
mesh = o3d.io.read_triangle_mesh(i[4].decode('UTF-8')) # i[4] -> mesh file path.
aabb = mesh.get_axis_aligned_bounding_box()
TotalVolume += np.prod(aabb.max_bound - aabb.min_bound)
# 4. Generating pointcloud
pc_xyz = np.empty((0, 3), dtype=float)
for i in partsInfo:
# Read mesh file of URDF.
mesh = o3d.io.read_triangle_mesh(i[4].decode('UTF-8'))
# If mesh has more than 8 triangles
if np.asarray(mesh.triangles).shape[0] >= 1:
# 4-a. Calculate local volume (part volume)
aabb = mesh.get_axis_aligned_bounding_box()
LocalVolume = np.prod(aabb.max_bound - aabb.min_bound)
# 4-b. Get point cloud from the part given number of points
PointsNum = max(int(5000 * (LocalVolume / TotalVolume)), 1000)
pcd_part = mesh.sample_points_poisson_disk(number_of_points=PointsNum, init_factor=2)
# pcd = pcd.voxel_down_sample(voxel_size=0.025)
# 4-c. Convert pointcloud to numpy array and concatenates all parts
pc_new = np.asarray(pcd_part.points)
pc_xyz = np.append(pc_xyz, pc_new, axis=0)
# Convert numpy pointcloud back to o3d type
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(pc_xyz)
return pcd
def get_obj_info(oID): #TODO: what about not mesh objects?
obj_data = p.getCollisionShapeData(oID, -1)[0]
geometry_type = obj_data[2]
#print("geometry type: " + str(geometry_type))
dimensions = obj_data[3]
#print("dimensions: "+ str(dimensions))
local_frame_pos = obj_data[5]
#print("local frome position: " + str(local_frame_pos))
local_frame_orn = obj_data[6]
#print("local frame oren: " + str(local_frame_orn))
diagonal = sqrt(dimensions[0]**2+dimensions[1]**2+dimensions[2]**2)
#print("diagonal: ", diagonal)
max_radius = diagonal/2
return local_frame_pos, max_radius
def reset(self):
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), [0, 0, 0], physicsClientId=self._physics_client_id)
# Load table and object
self.table_id = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "table/table.urdf"),
basePosition=[0.85, 0.0, 0.0], useFixedBase=True, physicsClientId=self._physics_client_id)
table_info = p.getCollisionShapeData(self.table_id, -1, physicsClientId=self._physics_client_id)[0]
self._h_table = table_info[5][2] + table_info[3][2]/2
# set ws limit on z according to table height
self._ws_lim[2][:] = [self._h_table, self._h_table + 0.3]
self.load_object(self._obj_name)
def get_obj_info(oID): #TODO: what about not mesh objects?
"""
get object data to figure out how far away the hand needs to be to make its approach
"""
obj_data = p.getCollisionShapeData(oID, -1)[0]
geometry_type = obj_data[2]
#print("geometry type: " + str(geometry_type))
dimensions = obj_data[3]
#print("dimensions: "+ str(dimensions))
local_frame_pos = obj_data[5]
#print("local frome position: " + str(local_frame_pos))
local_frame_orn = obj_data[6]
#print("local frame oren: " + str(local_frame_orn))
diagonal = sqrt(dimensions[0]**2 + dimensions[1]**2 + dimensions[2]**2)
#print("diagonal: ", diagonal)
max_radius = diagonal / 2
return local_frame_pos, max_radius
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]
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 i in range(len(visualShapes)):
v = visualShapes[i]
if (v[1] == linkIndex and v[2] == p.GEOM_MESH):
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
urdfVisual.geom_meshfilename = v[4].decode("utf-8")
urdfVisual.geom_meshscale = v[3]
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 i in range(len(collisionShapes)):
v = collisionShapes[i]
if (v[2] == p.GEOM_MESH):
urdfCollision = UrdfCollision()
urdfCollision.geom_type = v[2]
urdfCollision.geom_meshfilename = urdfLink.urdf_visual_shapes[
i].geom_meshfilename
urdfCollision.geom_meshscale = v[3]
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_world_collision_pose(body_unique_id: int, link_index: int):
"""Pose of collision 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 of ): See description.
"""
world_inertial_pose = get_world_inertial_pose(body_unique_id, link_index)
collision_shape_data = p.getCollisionShapeData(body_unique_id, link_index)
if len(collision_shape_data) > 1:
raise NotImplementedError
local_collision_pose = (collision_shape_data[0][5],
collision_shape_data[0][6])
return p.multiplyTransforms(world_inertial_pose[0], world_inertial_pose[1],
local_collision_pose[0],
local_collision_pose[1])
def InitModel(self):
# Map names to id
self.numJoints = p.getNumJoints(self.linkage)
logging.info("There are %d links in file\n", self.numJoints)
vis_data = p.getVisualShapeData(self.linkage)
for i in range(self.numJoints):
# map joint names to id (same mapping for links)
jointInfo = p.getJointInfo(self.linkage, i)
self.jointDict[jointInfo[1].decode('UTF-8')] = i
# record parent and child ids for each joint [child, parent]
# self.linkJoints.append([[jointInfo[16]], [i]])
# ignore the dummy link
# if(i > 0) :
# self.linkJoints[jointInfo[16]][self.LINK_PARENT_IDX].append(jointInfo[16])
# measure force/torque at the joints
p.enableJointForceTorqueSensor(self.linkage, i, enableSensor=True)
# Store initial positions and orientations
state = p.getLinkState(self.linkage, jointInfo[0])
self.initPosOrn.append([state[4], state[5]])
# debugging
coll_data = p.getCollisionShapeData(self.linkage, i)
logging.debug("%s %d", jointInfo[1].decode('UTF-8'), i)
logging.debug("com frame (posn (global) & orn (global)) %s %s",
str(state[0]),
str(p.getEulerFromQuaternion(state[1])))
logging.debug(
"inertial offset (posn (link frame), orn (link frame)) %s %s",
str(state[2]), str(p.getEulerFromQuaternion(state[3])))
logging.debug("link frame (posn (global), orn (global)) %s %s",
str(state[4]),
str(p.getEulerFromQuaternion(state[5])))
logging.debug("type %s", str(jointInfo[2]))
logging.debug("axis %s", str(jointInfo[13]))
logging.debug("collision (posn (COM frame), orn(COM frame)) %s %s",
str(coll_data[0][5]),
str(p.getEulerFromQuaternion(coll_data[0][6])))
logging.debug(
"visual (posn (link frame), orn(link frame)) %s %s\n\n",
str(vis_data[i][5]),
str(p.getEulerFromQuaternion(vis_data[i][6])))
# Create list of actuated joints
self.actJointIds = [
self.jointDict[act_name] for act_name in self.actJointNames
]
self.actJointNum = len(self.actJointNames)
# Create list of spherical joints
self.spherJointIds = [
self.jointDict[spher_name] for spher_name in self.spherJointNames
]
self.spherJointNum = len(self.spherJointNames)
# Create list of dumb joints
self.dumbJointIds = [
self.jointDict[dumb_name] for dumb_name in self.dumbJointNames
]
self.dumbJointNum = len(self.dumbJointNames)
# Init struct
self.linkVelo = [[0, 0, 0], [0, 0, 0]] * p.getNumJoints(self.linkage)
# Enable collision by default
self.ToggleCollision(1)
# Go to home position
self.HomePos()
# Constraints
for i in range(self.numConstraints):
parent_id = self.jointDict[self.constraintLinks[i][
self.CONS_PARENT_IDX]]
child_id = self.jointDict[self.constraintLinks[i][
self.CONS_CHILD_IDX]]
self.constraintDict[self.constraintLinks[i][
self.CONS_NAME_IDX]] = p.createConstraint(
self.linkage, parent_id, self.linkage, child_id,
self.constraintType, self.constraintAxis,
self.constraintParentPos[i], self.constraintChildPos)
p.changeConstraint(self.constraintDict[self.constraintLinks[i][
self.CONS_NAME_IDX]],
maxForce=self.CONSTRAINT_MAX_FORCE)
# # Add to linkJoint list (make constraints -ve ids)
# self.linkJoints[parent_id][self.LINK_PARENT_IDX].append(parent_id*-1)
# self.linkJoints[child_id][self.LINK_CHILD_IDX].append(child_id*-1)
# Set joint control
self.SetActJointControlType(self.POSITION_CONTROL)
self.ResetActJointControl()
self.ControlDumbJoints()
self.ControlSpherJoints()
# Gravity
p.setGravity(0, 0, self.GRAV_ACCELZ)
# Simulation type
self.SetSimType(self.useRealTime)
def get_object_shape_info(self):
info = list(p.getCollisionShapeData(self.obj_id, -1, physicsClientId=self._physics_client_id)[0])
info[4] = p.getVisualShapeData(self.obj_id, -1, physicsClientId=self._physics_client_id)[0][4]
return info
'/point_sample/pc_save.npy')
if NPYisExist:
continue
with suppress_stdout():
try:
ArticulatedObjId = pb.loadURDF(directory_path + data_num +
'/mobility.urdf', [0, 0, 0],
[0, 0, 0, 1],
useFixedBase=1)
except:
error_file.write(str(data_num) + "\n")
continue
URDFInfo = []
for jointId in range(pb.getNumJoints(ArticulatedObjId)):
jointInfo = pb.getCollisionShapeData(ArticulatedObjId, jointId)
for part in jointInfo:
URDFInfo.append([jointId, part])
with open(directory_path + data_num + '/result.json') as f:
json_file = json.load(f)
PartInfo = []
parse_json(json_file, PartInfo)
segment_match = {}
for j in PartInfo:
for i in URDFInfo:
for k in j['objs'][0]:
result = i[1][4].decode('utf-8').find(k + '.')
if result != -1:
urdf_flags = p.URDF_USE_INERTIA_FROM_FILE | p.URDF_MAINTAIN_LINK_ORDER
linkage = p.loadURDF("assembly.urdf",
startPos,
startOrn,
useFixedBase=1,
flags=urdf_flags)
# See startup info
numJoints = p.getNumJoints(linkage)
logging.info("There are %d links in file\n", numJoints)
jointInfoList = {}
for i in range(numJoints):
jointInfo = p.getJointInfo(linkage, i)
jointInfoList[jointInfo[1].decode('UTF-8')] = jointInfo[0]
state = p.getLinkState(linkage, jointInfo[0])
coll_data = p.getCollisionShapeData(linkage, jointInfo[0])
logging.debug("%s %d", jointInfo[1].decode('UTF-8'), i)
logging.debug("links: %s %d", jointInfo[12], jointInfo[16])
logging.debug("com frame (posn & orn) %s %s", str(state[0]),
str(p.getEulerFromQuaternion(state[1])))
logging.debug("inertial offset (posn & orn) %s %s", str(state[2]),
str(p.getEulerFromQuaternion(state[3])))
logging.debug("link frame (posn & orn) %s %s", str(state[4]),
str(p.getEulerFromQuaternion(state[5])))
logging.debug("type %s", str(jointInfo[2]))
logging.debug("collision (path, posn, orn) %s \n\n", str(coll_data))
# Set collision characteristics
p.setCollisionFilterPair(linkage, linkage,
jointInfoList['dogbone_joint_far_left'],
jointInfoList['left_eye_joint'], 1)
def sim_bullet(self, table, mode='GUI'):
if mode == "GUI":
self.mode = "GUI"
p.connect(
p.GUI, 1234
) # use build-in graphical user interface, p.DIRECT: pass the final results
p.resetDebugVisualizerCamera(
cameraDistance=0.45,
cameraYaw=-90.0,
cameraPitch=-89,
cameraTargetPosition=[1.55, 0.85, 1.4])
elif mode == "DIRECT":
p.connect(p.DIRECT)
else:
print("Input wrong simulation mode ")
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setGravity(0., 0., -9.81)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
path_robots = "/home/hszlyw/PycharmProjects/ahky/robots"
# tablesize [2.14 x 1.22]
file1 = os.path.join(path_robots, "models", "air_hockey_table",
"model.urdf")
# self.table = p.loadURDF(file, [1.7, 0.85, 0.117], [0, 0, 0.0, 1.0])
self.table = p.loadURDF(file1, list(table[:3]), list(table[3:]))
file2 = os.path.join(path_robots, "models", "puck", "model2.urdf")
# self.puck = p.loadURDF(file, puck_poses[0, 0:3], [0, 0, 0.0, 1.0], flags=p.URDF_USE_IMPLICIT_CYLINDER)
self.puck = p.loadURDF(file2,
self.init_pos, [0, 0, 0.0, 1.0],
flags=p.URDF_USE_INERTIA_FROM_FILE
or p.URDF_MERGE_FIXED_LINKS)
j_puck = self.get_joint_map(self.puck)
j_table = self.get_joint_map(self.table)
tableshape = p.getCollisionShapeData(self.table,
j_table.get(b'base_joint'))
puckshape = p.getCollisionShapeData(self.puck, -1)
p.changeDynamics(self.puck, -1, lateralFriction=1)
p.changeDynamics(self.puck, -1, restitution=1)
p.changeDynamics(self.puck, -1, linearDamping=self.damp)
# p.changeDynamics(self.puck, -1, restitution=0.8)
# load parameters
p.changeDynamics(self.table,
j_table.get(b'base_joint'),
lateralFriction=self.t_lateral_f)
p.changeDynamics(self.table,
j_table.get(b'base_down_rim_l'),
lateralFriction=self.four_side_rim_latf)
p.changeDynamics(self.table,
j_table.get(b'base_down_rim_r'),
lateralFriction=self.four_side_rim_latf)
p.changeDynamics(self.table,
j_table.get(b'base_down_rim_top'),
lateralFriction=1) # no collision
p.changeDynamics(self.table,
j_table.get(b'base_left_rim'),
lateralFriction=self.left_rim_f)
p.changeDynamics(self.table,
j_table.get(b'base_right_rim'),
lateralFriction=self.left_rim_f)
p.changeDynamics(self.table,
j_table.get(b'base_up_rim_l'),
lateralFriction=self.four_side_rim_latf)
p.changeDynamics(self.table,
j_table.get(b'base_up_rim_r'),
lateralFriction=self.four_side_rim_latf)
p.changeDynamics(self.table,
j_table.get(b'base_up_rim_top'),
lateralFriction=1) # no collision
p.changeDynamics(self.table,
j_table.get(b'base_joint'),
restitution=0.2)
p.changeDynamics(self.table,
j_table.get(b'base_down_rim_l'),
restitution=self.four_side_rim_res)
p.changeDynamics(self.table,
j_table.get(b'base_down_rim_r'),
restitution=self.four_side_rim_res)
p.changeDynamics(self.table,
j_table.get(b'base_down_rim_top'),
restitution=self.four_side_rim_res) # no collision
p.changeDynamics(self.table,
j_table.get(b'base_left_rim'),
restitution=self.left_rim_res)
p.changeDynamics(self.table,
j_table.get(b'base_right_rim'),
restitution=self.right_rim_res)
p.changeDynamics(self.table,
j_table.get(b'base_up_rim_l'),
restitution=self.four_side_rim_res)
p.changeDynamics(self.table,
j_table.get(b'base_up_rim_r'),
restitution=self.four_side_rim_res)
p.changeDynamics(self.table,
j_table.get(b'base_up_rim_top'),
restitution=self.four_side_rim_res) # no collision
# init puck
self.collision_filter(self.puck, self.table)
p.resetBasePositionAndOrientation(self.puck, self.init_pos,
[0, 0, 0, 1.])
p.resetBaseVelocity(self.puck,
linearVelocity=self.lin_vel,
angularVelocity=self.ang_vel)
# START
pre_pos = self.init_pos
pos = [pre_pos] # record position
vel = [self.lin_vel] # record velocity
t_sim = [0] # record time
t = 0
p.setTimeStep(1 / 120.)
while (np.abs(np.array(p.getBaseVelocity(self.puck)[0][:-1])) >
.1).any() and .115 < np.abs(
p.getBasePositionAndOrientation(self.puck)[0][2]) < 1.2:
# while True:
p.stepSimulation()
# time.sleep(1/120.)
# t += 1/120
t_sim.append(t)
new_pos = p.getBasePositionAndOrientation(self.puck)[0]
pos.append(new_pos)
vel.append(p.getBaseVelocity(self.puck)[0])
if mode == 'GUI':
p.addUserDebugLine(pre_pos,
new_pos,
lineColorRGB=[0.5, 0.5, 0.5],
lineWidth=3)
pre_pos = new_pos
p.disconnect()
return np.array(t_sim)[:, None], np.array(pos), np.array(vel)
def get_collision_data(body, link=BASE_LINK):
# TODO: try catch
return [CollisionShapeData(*tup) for tup in p.getCollisionShapeData(body, link, physicsClientId=CLIENT)]
