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())