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
