def pick_element(self, element):
"""Send movement and IO instructions to pick up fabrication element.
Parameter
----------
element : :class:`~rapid_clay_formations_fab.fab_data.FabricationElement`
Representation of fabrication element to pick up.
"""
self.send(compas_rrc.SetTool(self.pick_place_tool.name))
log.debug("Tool {self.pick_place_tool.name} set.")
self.send(compas_rrc.SetWorkObject(self.wobjs.pick))
log.debug(f"Work object {self.wobjs.pick} set.")
# start watch
self.send(compas_rrc.StartWatch())
# TODO: Use separate obj for pick elems?
vector = element.get_normal(
) * self.pick_place_tool.elem_pick_egress_dist
T = Translation(vector)
egress_frame = element.get_uncompressed_top_frame().transformed(T)
self.send(
MoveToRobtarget(
egress_frame,
self.EXTERNAL_AXES_DUMMY,
self.speed.travel,
self.zone.travel,
))
self.send(
MoveToRobtarget(
element.get_uncompressed_top_frame(),
self.EXTERNAL_AXES_DUMMY,
self.speed.pick_place,
self.zone.pick,
))
self.send(compas_rrc.WaitTime(self.pick_place_tool.needles_pause))
self.extend_needles()
self.send(compas_rrc.WaitTime(self.pick_place_tool.needles_pause))
self.send(
MoveToRobtarget(
egress_frame,
self.EXTERNAL_AXES_DUMMY,
self.speed.pick_place,
self.zone.pick,
motion_type=Motion.LINEAR,
))
self.send(compas_rrc.StopWatch())
return self.send(compas_rrc.ReadWatch())
def pick_bullet(self, pick_elem):
"""Send movement and IO instructions to pick up a clay cylinder.
Parameter
----------
pick_elem : :class:`~rapid_clay_formations_fab.fab_data.ClayBullet`
Representation of fabrication element to pick up.
"""
self.send(compas_rrc.SetTool(self.pick_place_tool.name))
log.debug("Tool {} set.".format(self.pick_place_tool.name))
self.send(compas_rrc.SetWorkObject(self.wobjs.pick))
log.debug("Work object {} set.".format(self.wobjs.pick))
# start watch
self.send(compas_rrc.StartWatch())
# TODO: Use separate obj for pick elems?
vector = pick_elem.get_normal(
) * self.pick_place_tool.elem_pick_egress_dist
T = Translation(vector)
egress_frame = pick_elem.get_uncompressed_top_frame().transformed(T)
self.send(
MoveToFrame(egress_frame, self.speed.travel, self.zone.travel))
self.send(
MoveToFrame(
pick_elem.get_uncompressed_top_frame(),
self.speed.precise,
self.zone.pick,
))
self.send(compas_rrc.WaitTime(self.pick_place_tool.needles_pause))
self.extend_needles()
self.send(compas_rrc.WaitTime(self.pick_place_tool.needles_pause))
self.send(
MoveToFrame(
egress_frame,
self.speed.precise,
self.zone.pick,
motion_type=Motion.LINEAR,
))
self.send(compas_rrc.StopWatch())
return self.send(compas_rrc.ReadWatch())
def place_element(self, element):
"""Send movement and IO instructions to place a fabrication element.
Uses `fab_conf` set up with command line arguments and configuration
file.
Parameters
----------
element : :class:`rapid_clay_formations_fab.fab_data.FabricationElement`
Element to place.
Returns
-------
:class:`compas_rrc.FutureResult`
Object which blocks while waiting for feedback from robot. Calling result on
this object will return the time the procedure took.
"""
log.debug(f"Location frame: {element.location}")
self.send(compas_rrc.SetTool(self.pick_place_tool.name))
log.debug(f"Tool {self.pick_place_tool.name} set.")
self.send(compas_rrc.SetWorkObject(self.wobjs.place))
log.debug(f"Work object {self.wobjs.place} set.")
# start watch
self.send(compas_rrc.StartWatch())
for trajectory in element.travel_trajectories:
self.execute_trajectory(
trajectory,
self.speed.travel,
self.zone.travel,
)
# Execute trajectories in place motion until the last
for trajectory in element.place_trajectories[:-1]:
self.execute_trajectory(trajectory,
self.speed.travel,
self.zone.travel,
stop_at_last=True)
# Before executing last place trajectory, retract the needles.
self.retract_needles()
# Wait to let needles retract fully.
self.send(compas_rrc.WaitTime(self.pick_place_tool.needles_pause))
# Last place motion
self.execute_trajectory(
element.place_trajectories[-1],
self.speed.pick_place,
self.zone.place,
stop_at_last=True,
motion_type=Motion.LINEAR,
)
self.execute_trajectory(
element.return_place_trajectories[0],
self.speed.pick_place,
self.zone.place,
)
for trajectory in element.return_place_trajectories[1:]:
self.execute_trajectory(trajectory, self.speed.travel,
self.zone.travel)
for trajectory in element.return_travel_trajectories:
self.execute_trajectory(
trajectory,
self.speed.travel,
self.zone.travel,
)
self.send(compas_rrc.StopWatch())
return self.send(compas_rrc.ReadWatch())
def fabrication(run_conf: confuse.AttrDict, run_data: dict) -> None:
"""Fabrication runner placing elements according to fab_data and conf."""
compose_up_driver(run_conf.robot_client.controller)
# setup fab data
fab_elements = run_data["fab_data"]
log.info(f"{len(fab_elements)} fabrication elements.")
pick_station = run_data["pick_station"]
run_data_path = run_conf.run_data_path
# this regex strips rotation numbers of file path, i.e test.log.01 --> test.log
_clean_file_name = re.sub(r"\.\d+$", "", run_data_path.name)
run_data_path = run_data_path.with_name(_clean_file_name)
# Uses RotatingFileHandler to do a rollover to keep old versions of run_data_path
_handler = RotatingFileHandler(run_data_path, maxBytes=1, backupCount=100)
_handler.doRollover()
_handler.close()
# Finally dump run_data again to not confuse user with an empty file
_write_run_data(run_data_path, run_data, fab_elements)
_edit_fab_data(fab_elements)
prev_elem: Optional[PlaceElement] = None
# Start abb client
with AbbRcfFabricationClient(run_conf.robot_client,
pick_station) as rob_client:
rob_client.ensure_connection()
# Confirm start on flexpendant
rob_client.confirm_start()
# Set speed, accel, tool, wobj and move to start pos
rob_client.pre_procedure()
i = 0
# Fabrication loop
for i, elem in enumerate(fab_elements):
if elem.skip:
continue
# Setup log message and flex pendant message
log_msg = f"{i}/{len(fab_elements) - 1}, id {elem.id_}."
log.info(f"Sending {log_msg}")
# Having this as an f-string should mean that the timestamp will
# be set when the PrintText command is sent
pendant_msg = f"{datetime.now().strftime('%H:%M')}: Executing {log_msg}"
rob_client.send(PrintTextNoErase(pendant_msg))
# Start clock and send instructions
rob_client.send(compas_rrc.StartWatch())
if (not elem.skip_pick_movement
and not run_conf.robot_client.skip_all_pick_movements):
rob_client.pick_element()
# Save cycle time from last run
# The main reason though is to stop the fabrication loop until
# confirmation that last loop finished. It is done between pick
# instructions and place instructions to (hopefully) make sure the
# robot always has instructions to execute
if prev_elem and prev_elem.cycle_time_future: # Is there a clock to check?
prev_elem.cycle_time = _wait_and_return_future(
prev_elem.cycle_time_future)
# TODO: Move sysexit to _wait_and_return_future here?
if not prev_elem.cycle_time: # If KeyboardInterrupt was raised
log.info(
"Exiting script, breaking loop and saving run_data.")
_write_run_data(run_data_path, run_data, fab_elements)
sys.exit(0)
cycle_time_msg = f"Last cycle time was: {prev_elem.cycle_time:0.0f}"
log.info(cycle_time_msg)
rob_client.send(PrintTextNoErase(cycle_time_msg))
prev_elem.time_placed = datetime.now().timestamp()
log.debug(f"Time prev elem was placed: {elem.time_placed}")
rob_client.place_element(elem)
rob_client.send(compas_rrc.StopWatch())
elem.cycle_time_future = rob_client.send(compas_rrc.ReadWatch())
# set placed to mark progress
elem.placed = True
# Write progress to json while waiting for robot
_write_run_data(run_data_path, run_data, fab_elements)
prev_elem = elem
# Wait on last element
if prev_elem and prev_elem.cycle_time_future:
prev_elem.cycle_time = _wait_and_return_future(
prev_elem.cycle_time_future)
# Write progress of last run of loop
# First figure out if the file should be labeled done though.
_placed_is_true = filter(lambda x: x.placed, fab_elements)
if len(list(_placed_is_true)) == len(fab_elements):
_file = run_data_path.with_name(run_data_path.name + ".99done")
else:
_file = run_data_path
_write_run_data(_file, run_data, fab_elements)
# Send robot to safe end position and close connection
rob_client.post_procedure()
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 test_speeds(run_conf, run_data):
rob_conf = run_conf.robot_client
# Compose up master, driver, bridge
compose_up_driver(rob_conf.controller)
pick_station = run_data["pick_station"]
# This context manager sets up RosClient, inherits from AbbClient
# with added fabrication specific methods
# On exit it unadvertises topics and stops RosClient
with AbbRcfFabricationClient(rob_conf, pick_station) as client:
# Sends ping (noop) and restarts container if no response
client.ensure_connection()
client.log_and_print("Confirm start by pressing play.")
# Confirm start on flexpendant
client.confirm_start()
# Sends move to start position, retracts needles and sets the defaults
# speed and acceleration values.
client.pre_procedure()
# 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.log_and_print(f"Speed override: {SPEED_OVERRIDE} %")
client.log_and_print(f"Max TCP speed: {SPEED_OVERRIDE} mm/s")
client.log_and_print(f"Acceleration: {ACCELERATION} %")
client.log_and_print(f"Acceleration ramp: {ACCELERATION_RAMP} %")
if PICK_ELEMENT:
# Sends instruction to pick up element from first position
# Subsequent calls picks up from next positions
client.pick_element()
client.send(compas_rrc.SetTool(client.pick_place_tool.name))
client.send(compas_rrc.SetWorkObject("wobj0"))
client.log_and_print(f"Tool: {TOOL}")
client.log_and_print(f"wobj: {WOBJ}")
if TEST_FRAMES:
for i in range(N_TIMES_REPEAT_LISTS):
speed = START_SPEED + SPEED_INCREMENTS * i
client.log_and_print(
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.log_and_print(f"Cycle took {cycle_time} s.")
if TEST_JOINT_POS:
for i in range(N_TIMES_REPEAT_LISTS):
speed = START_SPEED + SPEED_INCREMENTS * i
client.log_and_print(
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.log_and_print(f"Cycle took {cycle_time} s.")
def place_bullet(self, cylinder):
"""Send movement and IO instructions to place a clay cylinder.
Uses `fab_conf` set up with command line arguments and configuration
file.
Parameters
----------
cylinder : :class:`rapid_clay_formations_fab.fab_data.ClayBullet`
cylinder to place.
Returns
-------
:class:`compas_rrc.FutureResult`
Object which blocks while waiting for feedback from robot. Calling result on
this object will return the time the procedure took.
"""
log.debug(f"Location frame: {cylinder.location}")
self.send(compas_rrc.SetTool(self.pick_place_tool.name))
log.debug("Tool {} set.".format(self.pick_place_tool.name))
self.send(compas_rrc.SetWorkObject(self.wobjs.place))
log.debug("Work object {} set.".format(self.wobjs.place))
# start watch
self.send(compas_rrc.StartWatch())
for trajectory in cylinder.travel_trajectories:
self.execute_trajectory(
trajectory,
self.speed.travel,
self.zone.travel,
)
# Execute trajectories in place motion until the last
for trajectory in cylinder.place_trajectories[:-1]:
self.execute_trajectory(
trajectory,
self.speed.precise,
self.zone.travel,
)
# Before executing last place trajectory, retract the needles.
self.retract_needles()
# Wait to let needles retract fully.
self.send(compas_rrc.WaitTime(self.pick_place_tool.needles_pause))
# Last place motion
self.execute_trajectory(
cylinder.place_trajectories[-1],
self.speed.precise,
self.zone.place,
stop_at_last=True,
)
self.execute_trajectory(cylinder.return_place_trajectories[0],
self.speed.precise, self.zone.place)
# The last trajectory filtered down to only the last configuration
last_return_place_trajectory = cylinder.return_place_trajectories[-1]
last_return_trajectory_conf = last_return_place_trajectory.points[-1]
self.send(
MoveToJoints(
revolute_configuration_to_robot_joints(
last_return_trajectory_conf),
self.EXTERNAL_AXES_DUMMY,
self.speed.travel,
self.zone.travel,
))
for trajectory in cylinder.return_travel_trajectories:
self.execute_trajectory(
trajectory,
self.speed.travel,
self.zone.travel,
)
self.send(compas_rrc.StopWatch())
return self.send(compas_rrc.ReadWatch())