def module_setup(request): logging.loginfo(u'starting pybullet') pbw.start_pybullet(False) logging.loginfo(u'deleting tmp test folder') try: shutil.rmtree(folder_name) except: pass def kill_pybullet(): logging.loginfo(u'shutdown pybullet') pbw.stop_pybullet() request.addfinalizer(kill_pybullet)
def initialize_god_map(): god_map = GodMap() blackboard = Blackboard blackboard.god_map = god_map god_map.safe_set_data(identifier.rosparam, rospy.get_param(rospy.get_name())) god_map.safe_set_data(identifier.robot_description, rospy.get_param(u'robot_description')) path_to_data_folder = god_map.safe_get_data(identifier.data_folder) # fix path to data folder if not path_to_data_folder.endswith(u'/'): path_to_data_folder += u'/' god_map.safe_set_data(identifier.data_folder, path_to_data_folder) # fix nWSR nWSR = god_map.safe_get_data(identifier.nWSR) if nWSR == u'None': nWSR = None god_map.safe_set_data(identifier.nWSR, nWSR) pbw.start_pybullet(god_map.safe_get_data(identifier.gui)) while not rospy.is_shutdown(): try: controlled_joints = rospy.wait_for_message(u'/whole_body_controller/state', JointTrajectoryControllerState, timeout=5.0).joint_names except ROSException as e: logging.logerr(u'state topic not available') logging.logerr(e) else: break rospy.sleep(0.5) joint_weight_symbols = process_joint_specific_params(identifier.joint_weights, identifier.default_joint_weight_identifier, god_map) process_joint_specific_params(identifier.collisions_distances, identifier.default_collision_distances, god_map) joint_vel_symbols = process_joint_specific_params(identifier.joint_vel, identifier.default_joint_vel, god_map) joint_acc_symbols = process_joint_specific_params(identifier.joint_acc, identifier.default_joint_acc, god_map) world = PyBulletWorld(god_map.safe_get_data(identifier.gui), blackboard.god_map.safe_get_data(identifier.data_folder)) god_map.safe_set_data(identifier.world, world) robot = WorldObject(god_map.safe_get_data(identifier.robot_description), None, controlled_joints) world.add_robot(robot, None, controlled_joints, joint_vel_symbols, joint_acc_symbols, joint_weight_symbols, True, ignored_pairs=god_map.safe_get_data(identifier.ignored_self_collisions), added_pairs=god_map.safe_get_data(identifier.added_self_collisions), symengine_backend=god_map.safe_get_data(identifier.symengine_backend), symengine_opt_level=god_map.safe_get_data(identifier.symengine_opt_level)) joint_position_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(), identifier.joint_states, suffix=[u'position']) joint_vel_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(), identifier.joint_states, suffix=[u'velocity']) world.robot.reinitialize(joint_position_symbols.joint_map, joint_vel_symbols.joint_map) return god_map
def initialize_god_map(): god_map = GodMap() blackboard = Blackboard blackboard.god_map = god_map load_config_file() god_map.set_data(identifier.rosparam, rospy.get_param(rospy.get_name())) god_map.set_data(identifier.robot_description, rospy.get_param(u'robot_description')) path_to_data_folder = god_map.get_data(identifier.data_folder) # fix path to data folder if not path_to_data_folder.endswith(u'/'): path_to_data_folder += u'/' god_map.set_data(identifier.data_folder, path_to_data_folder) # fix nWSR nWSR = god_map.get_data(identifier.nWSR) if nWSR == u'None': nWSR = None god_map.set_data(identifier.nWSR, nWSR) pbw.start_pybullet(god_map.get_data(identifier.gui)) while not rospy.is_shutdown(): try: controlled_joints = rospy.wait_for_message(u'/whole_body_controller/state', JointTrajectoryControllerState, timeout=5.0).joint_names except ROSException as e: logging.logerr(u'state topic not available') logging.logerr(str(e)) else: break rospy.sleep(0.5) joint_weight_symbols = process_joint_specific_params(identifier.joint_weight, identifier.joint_weight_default, identifier.joint_weight_override, god_map) process_joint_specific_params(identifier.self_collision_avoidance_distance, identifier.self_collision_avoidance_default_threshold, identifier.self_collision_avoidance_default_override, god_map) process_joint_specific_params(identifier.external_collision_avoidance_distance, identifier.external_collision_avoidance_default_threshold, identifier.external_collision_avoidance_default_override, god_map) # TODO add checks to test if joints listed as linear are actually linear joint_velocity_linear_limit_symbols = process_joint_specific_params(identifier.joint_velocity_linear_limit, identifier.joint_velocity_linear_limit_default, identifier.joint_velocity_linear_limit_override, god_map) joint_velocity_angular_limit_symbols = process_joint_specific_params(identifier.joint_velocity_angular_limit, identifier.joint_velocity_angular_limit_default, identifier.joint_velocity_angular_limit_override, god_map) joint_acceleration_linear_limit_symbols = process_joint_specific_params(identifier.joint_acceleration_linear_limit, identifier.joint_acceleration_linear_limit_default, identifier.joint_acceleration_linear_limit_override, god_map) joint_acceleration_angular_limit_symbols = process_joint_specific_params( identifier.joint_acceleration_angular_limit, identifier.joint_acceleration_angular_limit_default, identifier.joint_acceleration_angular_limit_override, god_map) world = PyBulletWorld(False, blackboard.god_map.get_data(identifier.data_folder)) god_map.set_data(identifier.world, world) robot = WorldObject(god_map.get_data(identifier.robot_description), None, controlled_joints) world.add_robot(robot, None, controlled_joints, ignored_pairs=god_map.get_data(identifier.ignored_self_collisions), added_pairs=god_map.get_data(identifier.added_self_collisions)) joint_position_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_movable_joints(), identifier.joint_states, suffix=[u'position']) joint_vel_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_movable_joints(), identifier.joint_states, suffix=[u'velocity']) world.robot.update_joint_symbols(joint_position_symbols.joint_map, joint_vel_symbols.joint_map, joint_weight_symbols, joint_velocity_linear_limit_symbols, joint_velocity_angular_limit_symbols, joint_acceleration_linear_limit_symbols, joint_acceleration_angular_limit_symbols) world.robot.init_self_collision_matrix() return god_map