def post_procedure(client):
"""Post fabrication procedure, end pose and closing and termination of client.
Uses ``fab_conf`` set up using
:func:`compas_rcf.fab_data.interactive_conf_setup` for fabrication settings.
Parameters
----------
client : :class:`compas_rrc.AbbClient`
"""
grip_and_release(
client,
fab_conf["tool"]["io_needles_pin"].get(),
fab_conf["tool"]["release_state"].get(),
wait_before=fab_conf["tool"]["wait_before_io"].get(),
wait_after=fab_conf["tool"]["wait_after_io"].get(),
)
client.send(
MoveToJoints(
fab_conf["safe_joint_positions"]["end"].get(),
EXTERNAL_AXIS_DUMMY,
fab_conf["movement"]["speed_travel"].as_number(),
fab_conf["movement"]["zone_travel"].get(ZoneDataTemplate()),
))
client.send_and_wait(PrintText("Finished"))
# Close client
client.close()
client.terminate()
def post_procedure(self):
"""Post fabrication procedure, end pose and closing and termination of client.
Uses ``fab_conf`` set up using
:func:`compas_rcf.fab_data.interactive_conf_setup` for fabrication settings.
Parameters
----------
client : :class:`compas_rrc.AbbClient`
"""
self.retract_needles()
self.send(
MoveToJoints(
self.set_joint_pos.end,
self.EXTERNAL_AXIS_DUMMY,
self.speed.travel,
self.zone.travel,
))
self.send_and_wait(PrintText("Finished"))
def go_to_joint_pos(args: argparse.Namespace) -> None:
"""Send instruction to go to joint position.
Parameters
----------
args : :class:`argparse.Namespace`
"""
selection_instructions = {
"Calibration position": CALIBRATION_JOINT_POSITION,
"Travel position": TRAVEL_JOINT_POSITION,
}
compose_up_driver(args.controller)
selection = questionary.select(
"Select joint position to go to", selection_instructions.keys()
).ask()
log.debug(f"{selection} selected.")
with AbbRcfClient(ros_port=9090) as client:
client.ensure_connection()
client.send(SetAcceleration(ACCEL, ACCEL_RAMP))
client.send(SetMaxSpeed(SPEED_OVERRIDE, SPEED_MAX_TCP))
client.confirm_start()
log_msg = f"Moving to {selection.lower()}."
client.send(PrintText(log_msg))
log.info(log_msg)
client.send(
MoveToJoints(
selection_instructions[selection],
client.EXTERNAL_AXES_DUMMY,
SPEED,
ZONE,
)
)
def test_speeds():
with RosClient() as ros:
client = compas_rrc.AbbClient(ros)
client.run()
# Confirm start on flexpendant
client.send(PrintText("Confirm start by pressing play."))
client.send(compas_rrc.Stop())
# The speed and acceleration values are then overriden here
client.send(compas_rrc.SetMaxSpeed(SPEED_OVERRIDE, SPEED_MAX_TCP))
client.send(compas_rrc.SetAcceleration(ACCELERATION,
ACCELERATION_RAMP))
client.send(PrintText(f"Speed override: {SPEED_OVERRIDE} %"))
client.send(PrintText(f"Max TCP speed: {SPEED_OVERRIDE} mm/s"))
client.send(PrintText(f"Acceleration: {ACCELERATION} %"))
client.send(PrintText(f"Acceleration ramp: {ACCELERATION_RAMP} %"))
client.send(compas_rrc.SetTool(client.pick_place_tool.name))
client.send(compas_rrc.SetWorkObject("wobj0"))
client.send(PrintText(f"Tool: {TOOL}"))
client.send(PrintText(f"wobj: {WOBJ}"))
if TEST_JOINT_POS:
for i in range(N_TIMES_REPEAT_LISTS):
speed = START_SPEED + SPEED_INCREMENTS * i
client.send(
PrintText(f"Speed: {speed} mm/s, Zone: {ZONE} mm.", ))
client.send(compas_rrc.StartWatch())
for pos in JOINT_POS:
client.send(
MoveToJoints(pos, client.EXTERNAL_AXES_DUMMY, speed,
ZONE))
# Block until finished and print cycle time
client.send(compas_rrc.StopWatch())
cycle_time = client.send_and_wait(compas_rrc.ReadWatch(),
timeout=TIMEOUT)
client.send(PrintText(f"Cycle took {cycle_time} s."))
if TEST_FRAMES:
for i in range(N_TIMES_REPEAT_LISTS):
speed = START_SPEED + SPEED_INCREMENTS * i
client.send(
PrintText(f"Speed: {speed} mm/s, Zone: {ZONE} mm.", ))
client.send(compas_rrc.StartWatch())
for frame in FRAMES:
client.send(
MoveToFrame(
frame,
speed,
ZONE,
motion_type=MOTION_TYPE,
))
# Block until finished and print cycle time
client.send(compas_rrc.StopWatch())
cycle_time = client.send_and_wait(compas_rrc.ReadWatch(),
timeout=TIMEOUT)
client.send(PrintText(f"Cycle took {cycle_time} s."))
client.close()
def main():
"""Fabrication runner, sets conf, reads json input and runs fabrication process."""
############################################################################
# Docker setup #
############################################################################
ip = {"ROBOT_IP": ROBOT_IPS[fab_conf["target"].as_str()]}
compose_up(DOCKER_COMPOSE_PATHS["driver"], check_output=True, env_vars=ip)
log.debug("Driver services are running.")
############################################################################
# Load fabrication data #
############################################################################
fab_json_path = fab_conf["paths"]["fab_data_path"].as_path()
clay_bullets = load_bullets(fab_json_path)
log.info("Fabrication data read from: {}".format(fab_json_path))
log.info("{} items in clay_bullets.".format(len(clay_bullets)))
pick_station_json = fab_conf["paths"]["pick_conf_path"].as_path()
with pick_station_json.open(mode="r") as fp:
pick_station_data = json.load(fp)
pick_station = PickStation.from_data(pick_station_data)
############################################################################
# Create Ros Client #
############################################################################
ros = RosClient()
############################################################################
# Create ABB Client #
############################################################################
abb = AbbClient(ros)
abb.run()
log.debug("Connected to ROS")
check_reconnect(
abb,
driver_container_name=DRIVER_CONTAINER_NAME,
timeout_ping=fab_conf["docker"]["timeout_ping"].get(),
wait_after_up=fab_conf["docker"]["sleep_after_up"].get(),
)
############################################################################
# setup in_progress JSON #
############################################################################
if not fab_conf["skip_progress_file"]:
json_progress_identifier = "IN_PROGRESS-"
if fab_json_path.name.startswith(json_progress_identifier):
in_progress_json = fab_json_path
else:
in_progress_json = fab_json_path.with_name(
json_progress_identifier + fab_json_path.name)
############################################################################
# Fabrication loop #
############################################################################
to_place = setup_fab_data(clay_bullets)
if not questionary.confirm("Ready to start program?").ask():
log.critical("Program exited because user didn't confirm start.")
print("Exiting.")
sys.exit()
# Set speed, accel, tool, wobj and move to start pos
pre_procedure(abb)
for bullet in to_place:
bullet.placed = None
bullet.cycle_time = None
for i, bullet in enumerate(to_place):
current_bullet_desc = "Bullet {:03}/{:03} with id {}.".format(
i,
len(to_place) - 1, bullet.bullet_id)
abb.send(PrintText(current_bullet_desc))
log.info(current_bullet_desc)
pick_frame = pick_station.get_next_frame(bullet)
# Pick bullet
pick_future = pick_bullet(abb, pick_frame)
# Place bullet
place_future = place_bullet(abb, bullet)
bullet.placed = 1 # set placed to temporary value to mark it as "placed"
# Write progress to json while waiting for robot
if not fab_conf["skip_progress_file"].get():
with in_progress_json.open(mode="w") as fp:
json.dump(clay_bullets, fp, cls=ClayBulletEncoder)
log.debug("Wrote clay_bullets to {}".format(in_progress_json.name))
# This blocks until cycle is finished
cycle_time = pick_future.result() + place_future.result()
bullet.cycle_time = cycle_time
log.debug("Cycle time was {}".format(bullet.cycle_time))
bullet.placed = time.time()
log.debug("Time placed was {}".format(bullet.placed))
############################################################################
# Shutdown procedure #
############################################################################
# Write progress of last run of loop
if not fab_conf["skip_progress_file"].get():
with in_progress_json.open(mode="w") as fp:
json.dump(clay_bullets, fp, cls=ClayBulletEncoder)
log.debug("Wrote clay_bullets to {}".format(in_progress_json.name))
if (len([bullet for bullet in clay_bullets if bullet.placed is None]) == 0
and not fab_conf["skip_progress_file"].get()):
done_file_name = fab_json_path.name.replace(json_progress_identifier,
"")
done_json = fab_conf["paths"]["json_dir"].as_path(
) / "00_done" / done_file_name
in_progress_json.rename(done_json)
with done_json.open(mode="w") as fp:
json.dump(clay_bullets, fp, cls=ClayBulletEncoder)
log.debug("Saved placed bullets to 00_Done.")
elif not fab_conf["skip_progress_file"].get():
log.debug("Bullets without placed timestamp still present, keeping {}".
format(in_progress_json.name))
log.info("Finished program with {} bullets.".format(len(to_place)))
post_procedure(abb)
def run(run_conf, run_data):
"""Fabrication runner, sets conf, reads json input and runs fabrication process."""
############################################################################
# Docker setup #
############################################################################
# Publish TF static transform for transformation by ROS
if run_conf.publish_tf_xform:
if run_data.get("xform_path"):
log.debug(f"Loading matrix from {run_data['xform_path']}")
with open(run_data["xform_path"], mode="r") as fp:
xform = json.load(fp)
else:
# Publish identity matrix (zero matrix)
log.debug("Publishing zero matrix")
xform = Transformation()
publish_static_transform(xform,
scale_factor=1000) # mm to m scale factor
ip = {"ROBOT_IP": ROBOT_IPS[run_conf.robot_client.controller]}
compose_up(DOCKER_COMPOSE_PATHS["driver"], check_output=True, env_vars=ip)
log.debug("Driver services are running.")
############################################################################
# Load fabrication data #
############################################################################
clay_cylinders = [
ClayBullet.from_data(data) for data in run_data["fab_data"]
]
log.info("Fabrication data read.")
log.info(f"{len(clay_cylinders)} items in clay_cylinders.")
# Integrate into AbbRcfClient?
with run_conf.pick_conf.open(mode="r") as fp:
pick_station = PickStation.from_data(json.load(fp))
############################################################################
# setup in_progress JSON #
############################################################################
json_progress_identifier = "-IN_PROGRESS"
run_data_path = run_conf.run_data_path
if run_data_path.stem.endswith(
json_progress_identifier) or run_data_path.stem.endswith(
json_progress_identifier + ".json"):
progress_file = run_data_path
else:
progress_file = run_data_path.with_name(run_data_path.stem +
json_progress_identifier +
run_data_path.suffix)
i = 1
while progress_file.exists():
progress_file = progress_file / ".{:02}".format(i)
done_file = progress_file.with_name(
str(progress_file.name).replace(json_progress_identifier, "-DONE"))
# Create Ros Client #
with RosClient() as ros:
# Create AbbRcf client (subclass of AbbClient)
rob_client = AbbRcfClient(ros, run_conf.robot_client)
rob_client.check_reconnect()
############################################################################
# Fabrication loop #
############################################################################
to_place = setup_fab_data(clay_cylinders)
if not questionary.confirm("Ready to start program?").ask():
log.critical("Program exited because user didn't confirm start.")
print("Exiting.")
sys.exit()
# Set speed, accel, tool, wobj and move to start pos
rob_client.pre_procedure()
for bullet in to_place:
bullet.placed = None
bullet.cycle_time = None
for i, bullet in enumerate(to_place):
current_bullet_desc = "Bullet {:03}/{:03} with id {}.".format(
i,
len(to_place) - 1, bullet.bullet_id)
rob_client.send(PrintText(current_bullet_desc))
log.info(current_bullet_desc)
pick_frame = pick_station.get_next_frame(bullet)
# Pick bullet
pick_future = rob_client.pick_bullet(pick_frame)
# Place bullet
place_future = rob_client.place_bullet(bullet)
bullet.placed = 1 # set placed to temporary value to mark it as "placed"
# Write progress to json while waiting for robot
run_data["fab_data"] = [
cylinder.to_data() for cylinder in clay_cylinders
]
with progress_file.open(mode="w") as fp:
json.dump(run_data, fp, cls=ClayBulletEncoder)
log.debug("Wrote clay_bullets to {}".format(progress_file.name))
# This blocks until cycle is finished
cycle_time = pick_future.result() + place_future.result()
bullet.cycle_time = cycle_time
log.debug("Cycle time was {}".format(bullet.cycle_time))
bullet.placed = time.time()
log.debug("Time placed was {}".format(bullet.placed))
############################################################################
# Shutdown procedure #
############################################################################
# Write progress of last run of loop
run_data["fab_data"] = [
cylinder.to_data() for cylinder in clay_cylinders
]
with progress_file.open(mode="w") as fp:
json.dump(run_data, fp, cls=ClayBulletEncoder)
log.debug("Wrote run_data to {}".format(progress_file.name))
if len([bullet for bullet in clay_cylinders
if bullet.placed is None]) == 0:
progress_file.rename(done_file)
log.debug(f"Saved placed bullets to {done_file}.")
rob_client.post_procedure()
def std_move_to_frame(
frame,
tool="tool0",
wobj="wobj0",
motion_type=Motion.LINEAR,
speed=200,
accel=100,
zone=Zone.FINE,
timeout=None,
):
"""Move robot arm to target or targets.
Parameters
----------
frame : :class:`compas.geometry.Frame` or :obj:`list` of :class:`compas.geometry.Frame`
Target frame or frames.
tool : :obj:`str`
Name of tool as named in RAPID code on controller to use for TCP data.
wobj : :obj:`str`
Name of work object as named in RAPID code on controller to use for
coordinate system.
motion_type : :class:`compas_rrc.Motion`
Motion type, either linear (:class:`~compas_rrc.Motion.LINEAR`) or
joint (:class:`~compas_rrc.Motion.JOINT`).
speed : :obj:`float`
TCP speed in mm/s. Limited by hard coded max speed in this function as
well as safety systems on controller.
accel : :obj:`float`
Acceleration in percentage of standard acceleration.
zone : :class:`compas_rrc.Zone`
Set zone value of movement, (acceptable deviation from target).
timeout : :obj:`float`
Time to wait for indication of finished movement. If not defined no
feedback will be requested.
""" # noqa: E501
if not isinstance(frame, Sequence):
frames = [frame]
else:
frames = frame
with RosClient() as ros:
# Create ABB Client
abb = AbbClient(ros)
abb.run(timeout=5)
abb.send(SetTool(tool))
abb.send(SetWorkObject(wobj))
# Set acceleration data
ramp = 100 # % Higher values makes the acceleration ramp longer
abb.send(SetAcceleration(accel, ramp))
# Set speed override and max speed
speed_override = 100 # %
max_tcp_speed = 500 # mm/s
abb.send(SetMaxSpeed(speed_override, max_tcp_speed))
# Move to Frame
for f in frames:
abb.send(
PrintText(" Moving to {:.3f}, {:.3f}, {:.3f}.".format(
*f.point.data)))
abb.send(MoveToFrame(f, speed, zone, motion_type))
if timeout:
abb.send_and_wait(Noop(), timeout=timeout)
def fab_run(run_conf, run_data):
"""Fabrication runner, sets conf, reads json input and runs fabrication process."""
# Publish TF static transform for transformation by ROS
if run_conf.publish_tf_xform:
_publish_tf_static_xform(xform=run_data.get("xform"))
compose_up_driver(run_conf.robot_client.controller)
# setup fab data
fab_elements = [
ClayBullet.from_data(data) for data in run_data["fab_data"]
]
log.info("Fabrication data read.")
log.info(f"{len(fab_elements)} fabrication elements..")
# Setup pick station
with run_conf.pick_conf.open(mode="r") as fp:
pick_station = PickStation.from_data(json.load(fp))
log.info(f"Pick station setup read from {run_conf.pick_conf}")
run_data_path = run_conf.run_data_path
# setup in_progress JSON
progress_identifier = "-IN_PROGRESS"
progress_identifier_regex = re.compile(progress_identifier + r"\d{0,2}")
if progress_identifier in run_data_path.stem:
progress_file = run_data_path
i = 1
# Add number and increment it if needed.
while progress_file.exists():
# strip suffix
stem = progress_file.stem
# Match IN_PROGRESS with or without digits after and add/replace digits
new_name = re.sub(progress_identifier_regex,
progress_identifier + f"{i:02}", stem)
new_name += progress_file.suffix
progress_file = progress_file.with_name(new_name)
i += 1
else:
progress_file = run_data_path.with_name(run_data_path.stem +
progress_identifier +
run_data_path.suffix)
log.info(f"Progress will be saved to {progress_file}.")
done_file_name = re.sub(progress_identifier_regex, "-DONE",
progress_file.name)
done_file = progress_file.with_name(done_file_name)
# Fabrication loop
_edit_fab_data(fab_elements, run_conf)
# Create Ros Client
with RosClient(port=9090) as ros:
# Create AbbRcf client (subclass of AbbClient)
rob_client = AbbRcfClient(ros, run_conf.robot_client)
confirm_start()
rob_client.check_reconnect()
# Set speed, accel, tool, wobj and move to start pos
rob_client.pre_procedure()
# Initialize this before first run, it gets set after placement
cycle_time_msg = None
for i, elem in enumerate(fab_elements):
if elem.placed:
continue
current_elem_desc = f"{i}/{len(fab_elements) - 1}, id {elem.bullet_id}."
log.info(current_elem_desc)
pendant_msg = ""
if cycle_time_msg:
pendant_msg += cycle_time_msg + " "
pendant_msg += current_elem_desc
# TP write limited to 40 char / line
rob_client.send(PrintText(pendant_msg[:40]))
pick_frame = pick_station.get_next_frame(elem)
# Send instructions and store feedback obj
pick_future = rob_client.pick_bullet(pick_frame)
place_future = rob_client.place_bullet(elem)
elem.placed = True # set placed to temporary value to mark it as "placed"
# Write progress to json while waiting for robot
run_data["fab_data"] = fab_elements
with progress_file.open(mode="w") as fp:
json.dump(run_data, fp, cls=CompasObjEncoder)
log.debug(f"Wrote fabrication data to {progress_file.name}")
# This blocks until cycle is finished
cycle_time = pick_future.result() + place_future.result()
elem.cycle_time = cycle_time
# format float to int to save characters on teach pendant
cycle_time_msg = f"Last cycle time: {elem.cycle_time:0.0f}"
log.info(cycle_time_msg)
elem.time_placed = time.time()
log.debug(f"Time placed was {elem.time_placed}")
# Write progress of last run of loop
run_data["fab_data"] = fab_elements
with done_file.open(mode="w") as fp:
json.dump(run_data, fp, cls=CompasObjEncoder)
log.info("Wrote final run_data to {}".format(done_file.name))
rob_client.post_procedure()
def fab_run(run_conf, run_data):
"""Fabrication runner placing elements according to fab_data and conf."""
_compose_up_driver(run_conf.robot_client.controller)
# setup fab data
fab_elements = [FabricationElement.from_data(data) for data in run_data["fab_data"]]
log.info("Fabrication data read.")
log.info(f"{len(fab_elements)} fabrication elements.")
# Setup pick station
with run_conf.pick_conf.open(mode="r") as fp:
pick_station = PickStation.from_data(json.load(fp))
log.info(f"Pick station setup read from {run_conf.pick_conf}")
progress_file, done_file = _setup_file_paths(run_conf.run_data_path)
_edit_fab_data(fab_elements, run_conf)
# Fabrication loop
# Create Ros Client
with RosClient(port=9090) as ros:
# Create AbbRcf client (subclass of AbbClient)
rob_client = AbbRcfClient(ros, run_conf.robot_client)
rob_client.check_reconnect()
# Set speed, accel, tool, wobj and move to start pos
rob_client.pre_procedure()
# Confirm start on flexpendant
rob_client.confirm_start()
# Initialize this before first run, it gets set after placement
cycle_time_msg = None
for i, elem in enumerate(fab_elements):
if elem.placed: # Don't place elements marked as placed
continue
# Setup log message and flex pendant message
current_elem_desc = f"{i}/{len(fab_elements) - 1}, id {elem.id_}."
log.info(current_elem_desc)
pendant_msg = ""
if cycle_time_msg:
pendant_msg += cycle_time_msg + " "
pendant_msg += current_elem_desc
# TP write limited to 40 char / line
rob_client.send(PrintText(pendant_msg[:40]))
pick_element = pick_station.get_next_pick_elem(elem)
# Send instructions and store feedback obj
pick_future = rob_client.pick_element(pick_element)
place_future = rob_client.place_element(elem)
# set placed to temporary value to mark it as "placed"
elem.placed = True
# Write progress to json while waiting for robot
run_data["fab_data"] = fab_elements
with progress_file.open(mode="w") as fp:
json.dump(run_data, fp, cls=CompasObjEncoder)
log.debug(f"Wrote fabrication data to {progress_file.name}")
# This blocks until cycle is finished
cycle_time = pick_future.result() + place_future.result()
elem.cycle_time = cycle_time
# format float to int to save characters on teach pendant
cycle_time_msg = f"Last cycle time: {elem.cycle_time:0.0f}"
log.info(cycle_time_msg)
elem.time_placed = time.time()
log.debug(f"Time placed was {elem.time_placed}")
# Write progress of last run of loop
run_data["fab_data"] = fab_elements
with done_file.open(mode="w") as fp:
json.dump(run_data, fp, cls=CompasObjEncoder)
log.info("Wrote final run_data to {}".format(done_file.name))
# Send robot to safe end position and close connection
rob_client.post_procedure()