def configuration_from_json(): # define filepath filename = "assignment_03.json" directory = "C:/Users/trtku/OneDrive/Data/03_MAS/01_github/compas_fs2021/compas_2_local/lecture_04/ko_tsuruta" filepath = os.path.join(directory, filename) # translate json into deserialisable data desirial_data = read_data_from_json(filepath) # get configurations from json desirial_configs = [ Configuration.from_data(data) for data in desirial_data ] return desirial_configs
def test_from_data(): config = Configuration.from_data( dict(values=[8.312, 1.5], types=[Joint.PRISMATIC, Joint.REVOLUTE])) assert str(config) == 'Configuration((8.312, 1.500), (2, 0))'
HERE = os.path.dirname(__file__) DATA = os.path.abspath(os.path.join(HERE, "..", "data")) PATH_TO = os.path.join( DATA, os.path.splitext(os.path.basename(__file__))[0] + ".json") # Load assembly settings and create instances settings_file = os.path.join(DATA, "settings.json") with open(settings_file, 'r') as f: data = json.load(f) width, length, height = data['brick_dimensions'] tolerance_vector = Vector.from_data(data['tolerance_vector']) savelevel_vector = Vector.from_data(data['savelevel_vector']) brick = Element.from_data(data['brick']) start_configuration = Configuration.from_data(data['start_configuration']) picking_frame = Frame.from_data(data['picking_frame']) savelevel_picking_frame = picking_frame.copy() savelevel_picking_frame.point += savelevel_vector picking_frame.point += tolerance_vector # create tool from json filepath = os.path.join(DATA, "vacuum_gripper.json") tool = Tool.from_json(filepath) # load assembly from file filepath = os.path.join(DATA, '048_flemish_bond.json') # load from existing if calculation failed at one point... clear_planning_scene = True
# create Element and move it to target frame element = Element.from_data(data['brick']) # create Assembly with element at target_frame assembly = Assembly() T = Transformation.from_frame_to_frame(element.frame, target_frame) assembly.add_element(element.transformed(T)) # Bring the element's mesh into the robot's tool0 frame element_tool0 = element.copy() T = Transformation.from_frame_to_frame(element_tool0.gripping_frame, tool.frame) element_tool0.transform(T) # define picking_configuration picking_configuration = Configuration.from_data(data['picking_configuration']) # define picking frame picking_frame = Frame.from_data(data['picking_frame']) picking_frame.point += tolerance_vector # define savelevel frames 'above' the picking- and target frames savelevel_picking_frame = picking_frame.copy() savelevel_picking_frame.point += savelevel_vector savelevel_target_frame = target_frame.copy() savelevel_target_frame.point += savelevel_vector # settings for plan_motion tolerance_position = 0.001 tolerance_axes = [math.radians(1)] * 3
def local_executor(cls, options, host='127.0.0.1', port=19997): with VrepClient(debug=options.get('debug', True), host=host, port=port) as client: active_robot_options = None # Setup all robots' start state for r in options['robots']: robot = Robot(r['robot'], client) if 'start' in r: if r['start'].get('joint_values'): start = Configuration.from_data(r['start']) elif r['start'].get('values'): start = Frame.from_data(r['start']) try: reachable_state = client.inverse_kinematics(robot, start, metric_values=[0.] * robot.dof, max_trials=1, max_results=1) start = reachable_state[-1] LOG.info('Robot state found for start pose. External axes=%s, Joint values=%s', str(start.external_axes), str(start.joint_values)) except VrepError: raise ValueError('Start plane is not reachable: %s' % str(r['start'])) client.set_robot_config(robot, start) if 'building_member' in r: client.add_building_member(robot, Mesh.from_data(r['building_member'])) if 'goal' in r: active_robot_options = r # Set global scene options if 'collision_meshes' in options: client.add_meshes(map(Mesh.from_data, options['collision_meshes'])) # Check if there's at least one active robot (i.e. one with a goal defined) if active_robot_options: robot = Robot(active_robot_options['robot'], client) if active_robot_options['goal'].get('values'): goal = Pose.from_data(active_robot_options['goal']) else: raise ValueError('Unsupported goal type: %s' % str(active_robot_options['goal'])) kwargs = {} kwargs['metric_values'] = active_robot_options.get('metric_values') kwargs['planner_id'] = options.get('planner_id') kwargs['resolution'] = options.get('resolution') if 'joint_limits' in active_robot_options: joint_limits = active_robot_options['joint_limits'] if joint_limits.get('gantry'): kwargs['gantry_joint_limits'] = [item for sublist in joint_limits.get('gantry') for item in sublist] if joint_limits.get('arm'): kwargs['arm_joint_limits'] = [item for sublist in joint_limits.get('arm') for item in sublist] kwargs['trials'] = options.get('trials') kwargs['shallow_state_search'] = options.get('shallow_state_search') kwargs['optimize_path_length'] = options.get('optimize_path_length') # Filter None values kwargs = {k: v for k, v in kwargs.iteritems() if v is not None} path = client.plan_motion(robot, goal, **kwargs) LOG.info('Found path of %d steps', len(path)) else: robot = Robot(options['robots'][0]['robot'], client) path = [client.get_robot_config(robot)] return path