def record_poses(args: argparse.Namespace) -> None:
"""Program recording poses for localization procedure."""
log.info(
"Move to robot when prompted, press play on the pendant to record.")
log.info("Press CTRL+C when you are done.")
warn_about_scipy_fortran_ctrl_c()
log.info("Starting ROS ABB Driver")
compose_up_driver(args.controller)
poses: typing.List[compas.geometry.Frame] = []
file_name = datetime.now().strftime(STRFTIME_FMT) + ".json"
output_file = OUTPUT_DIR / file_name
output_file.touch()
with AbbRcfClient() as client:
client.send(compas_rrc.SetTool("t_A057_ClayTool02_Prism"))
client.send(compas_rrc.SetWorkObject("wobj0"))
client.ensure_connection()
while True:
client.send(
PrintTextNoErase(
f"Move robot to localization point {len(poses)+1}"))
client.send(PrintTextNoErase("Press play to record position."))
client.send(StopAll())
pose_future = client.send(compas_rrc.GetFrame())
try:
# Wait for result
# Sleeping allows catching KeyboardInterrupt
while pose_future.done is False:
time.sleep(3)
except KeyboardInterrupt:
log.info("Exiting script.")
break
pose = pose_future.result()
if pose in poses:
error_msg = "Position already recorded."
client.send(PrintTextNoErase(error_msg))
log.warning(error_msg)
continue
poses.append(pose)
log.info(f"Pose {len(poses)} recorded.")
# Write on every loop for safety
with output_file.open(mode="w") as fp:
json.dump(poses, fp, cls=compas.utilities.DataEncoder)
log.debug(f"Saved frames to {output_file}")
client.send(PrintTextNoErase(f"Position {len(poses)} recorded."))
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 pick_element(self) -> None:
"""Send movement and IO instructions to pick up fabrication element."""
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.pick))
log.debug(f"Work object {self.wobjs.pick} set.")
element = self.pick_station.get_next_pick_elem()
self.send(
MoveToRobtarget(
element.get_egress_frame(),
self.EXTERNAL_AXES_DUMMY,
self.speed.travel,
self.zone.travel,
))
self.send(
MoveToRobtarget(
element.get_top_frame(),
self.EXTERNAL_AXES_DUMMY,
self.speed.pick,
compas_rrc.Zone.FINE,
))
self.extend_needles()
self.send(compas_rrc.WaitTime(self.pick_place_tool.needles_pause))
self.send(
MoveToRobtarget(
element.get_egress_frame(),
self.EXTERNAL_AXES_DUMMY,
self.speed.pick,
self.zone.pick,
motion_type=Motion.LINEAR,
))
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 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_element(self, element: PlaceElement) -> compas_rrc.FutureResult:
"""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.PlaceElement`
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.")
for trajectory in self._get_travel_trajectories(element):
self.execute_trajectory(
trajectory,
self.speed.travel,
self.zone.travel,
)
place_trajectories = self._get_place_trajectories(element)
# Go to egress frame
self.execute_trajectory(
place_trajectories[0],
self.speed.travel,
self.zone.place_egress,
# Stop if wait_at_place_egress is larger than 0
stop_at_last=self.rob_conf.wait_at_place_egress > 0,
)
# Wait here if wait_at_place_egress is not 0
self.send(compas_rrc.WaitTime(self.rob_conf.wait_at_place_egress))
# Execute all trajectories between first and last
for trajectory in place_trajectories[1:-1]:
self.execute_trajectory(trajectory,
self.speed.travel,
self.zone.place_egress,
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
place_future = self.execute_trajectory(
place_trajectories[-1],
self.speed.place,
self.zone.place,
motion_type=Motion.LINEAR,
)
return_place_trajectories = self._get_return_place_trajectories(
element)
self.execute_trajectory(
return_place_trajectories[0],
self.speed.place,
self.zone.place,
)
for trajectory in return_place_trajectories[1:]:
self.execute_trajectory(trajectory, self.speed.travel,
self.zone.travel)
return_travel_trajectories = self._get_return_travel_trajectories(
element)
for trajectory in return_travel_trajectories:
self.execute_trajectory(
trajectory,
self.speed.travel,
self.zone.travel,
)
return place_future
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())