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()))