def usage():
logging.logdebug("Tests for kdl_parser:\n")
logging.logdebug("kdl_parser <urdfs file>")
logging.logdebug("\tLoad the URDF from file.")
logging.logdebug("kdl_parser")
logging.logdebug("\tLoad the URDF from the parameter server.")
sys.exit(1)
def load_urdf_string_into_bullet(urdf_string, pose=None):
"""
Loads a URDF string into the bullet world.
:param urdf_string: XML string of the URDF to load.
:type urdf_string: str
:param pose: Pose at which to load the URDF into the world.
:type pose: Pose
:return: internal PyBullet id of the loaded urdfs
:rtype: intload_urdf_string_into_bullet
"""
if pose is None:
pose = Pose()
pose.orientation.w = 1
if isinstance(pose, PoseStamped):
pose = pose.pose
object_name = URDFObject(urdf_string).get_name()
if object_name in get_body_names():
raise DuplicateNameException(
u'an object with name \'{}\' already exists in pybullet'.format(
object_name))
resolved_urdf = resolve_ros_iris_in_urdf(urdf_string)
filename = write_to_tmp(u'{}.urdfs'.format(random_string()), resolved_urdf)
with NullContextManager(
) if giskardpy.PRINT_LEVEL == DEBUG else suppress_stdout():
id = p.loadURDF(filename,
[pose.position.x, pose.position.y, pose.position.z], [
pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w
],
flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
os.remove(filename)
logging.logdebug(u'--> added {} to pybullet'.format(object_name))
return id
def post_tick(snapshot_visitor, behaviour_tree):
logging.logdebug(u'\n' + py_trees.display.ascii_tree(behaviour_tree.root,
snapshot_information=snapshot_visitor))
def suicide(self):
if self._pybullet_id is not None:
pw.removeBody(self._pybullet_id)
self._pybullet_id = None
logging.logdebug(u'<-- removed {} from pybullet'.format(self.get_name()))