def __init__( self, place_position, force_threshold=-8, hover_distance=0.12, step_size=0.05, ): """ @param place_position: Place position as [x, y, z] @param force_threshold: Z force threshold in Newtons @param hover_distance: Distance above the pose in meters @param step_size: Step size for approaching the pose """ self.place_position = place_position self.force_threshold = force_threshold self._hover_distance = hover_distance self.step_size = step_size self.robot = Robot() homedir = os.path.join(os.path.expanduser('~'), "grasp-comms") self.grasp_request = os.path.join(homedir, "grasp_request.npy") self.grasp_available = os.path.join(homedir, "grasp_available.npy") self.grasp_pose = os.path.join(homedir, "grasp_pose.npy")
def __init__(self, robot: Robot, use_lex=True, destination=None): self.fuzzy_system = MooFuzzySystem(use_lex=use_lex) self.r_destination = { 'x': 5, 'y': 5, 'theta': 0, } if destination is None else destination # initialize self.goal_threshold = 0.5 self.r = robot self.pos = robot.get_position()
import os os.environ["VREP"] = "/home/anjd/github/V-REP_PRO_EDU_V3_5_0_Linux" os.environ[ "VREP_LIBRARY"] = "/home/anjd/github/V-REP_PRO_EDU_V3_5_0_Linux/programming/remoteApiBindings/lib/lib/Linux/64Bit/" from pyrep import VRep from hardware.robot import Robot from moo_system import MooSystem from naive_system import NaiveSystem from simple_system import SimpleSystem if __name__ == '__main__': # run with VRep.connect("127.0.0.1", 19997) as api: r = Robot(api) # cs = NaiveSystem(r) # cs = SimpleSystem(r) cs = MooSystem(r) cs.run()
def exit_handler(): print('\nTerminating Program') cam.closeCamera() Robot.getInstance().turnOff() print('Program Terminated')
# starts robot with line following controller without the server import atexit from hardware.robot import Robot import hardware.camera as cam from autonomy.controllers.linefollowing import LineFollowing robot = Robot.getInstance() # called on exit to clean up def exit_handler(): print('\nTerminating Program') cam.closeCamera() Robot.getInstance().turnOff() print('Program Terminated') atexit.register(exit_handler) robot.setController(LineFollowing()) robot.turnOn() # robot runs in a seperate daemon thread # while true stops program from ending instantly while True: pass
def __init__(self): self.robot = Robot() homedir = os.path.join(os.path.expanduser('~'), "grasp-comms") self.move_completed = os.path.join(homedir, "move_completed.npy") self.tool_position = os.path.join(homedir, "tool_position.npy")
class Calibration: def __init__(self): self.robot = Robot() homedir = os.path.join(os.path.expanduser('~'), "grasp-comms") self.move_completed = os.path.join(homedir, "move_completed.npy") self.tool_position = os.path.join(homedir, "tool_position.npy") def run(self): # Connect to robot self.robot.connect() # Move robot to home pose print('Moving to start position...') self.robot.go_home() # Calibrate gripper self.robot.calibrate_gripper() # Allow user to install the checker board self.robot.open_gripper() raw_input('Press enter to close gripper..') self.robot.close_gripper() raw_input('Press enter to continue..') # Move robot to each calibration point in workspace print('Collecting data...') while not rospy.is_shutdown(): if not np.load(self.move_completed): tool_position = np.load(self.tool_position) print('Moving to tool position: ', tool_position) pose = get_pose(position=tool_position) pose_rot = rotate_pose_msg_by_euler_angles(pose, 0, np.pi/2, -np.pi/2) self.robot.move_to(pose_rot) np.save(self.move_completed, 1) else: time.sleep(0.1)
class PickAndPlace: def __init__( self, place_position, force_threshold=-8, hover_distance=0.12, step_size=0.05, ): """ @param place_position: Place position as [x, y, z] @param force_threshold: Z force threshold in Newtons @param hover_distance: Distance above the pose in meters @param step_size: Step size for approaching the pose """ self.place_position = place_position self.force_threshold = force_threshold self._hover_distance = hover_distance self.step_size = step_size self.robot = Robot() homedir = os.path.join(os.path.expanduser('~'), "grasp-comms") self.grasp_request = os.path.join(homedir, "grasp_request.npy") self.grasp_available = os.path.join(homedir, "grasp_available.npy") self.grasp_pose = os.path.join(homedir, "grasp_pose.npy") def _approach(self, pose): """ Move to a pose with a hover-distance above the requested pose and then move to the pose incrementally while monitoring the z force """ print('approaching...') approach = copy.deepcopy(pose) approach.position.z = approach.position.z + self._hover_distance self.robot.move_to(approach) while approach.position.z >= pose.position.z: approach.position.z = approach.position.z - self.step_size self.robot.move_to(approach, timeout=1.0) force = self.robot.get_force() print('force: ', self.robot.get_force()) if force.z < self.force_threshold: print(("End Effector Force is: " + str([force.x, force.y, force.z]))) print("Max z force reached before reaching the pose") break def _retract(self): """ Retract up from current pose """ # retrieve current pose from endpoint current_pose = self.robot.current_pose() pose = Pose() pose.position.x = current_pose['position'].x pose.position.y = current_pose['position'].y pose.position.z = current_pose['position'].z + self._hover_distance pose.orientation.x = current_pose['orientation'].x pose.orientation.y = current_pose['orientation'].y pose.orientation.z = current_pose['orientation'].z pose.orientation.w = current_pose['orientation'].w # servo up from current pose self.robot.move_to(pose) def pick(self, grasp_pose): """ Pick from given pose """ # Calculate grasp pose pose = get_pose(position=grasp_pose[:3]) # Apply grasp angle from model output pose = rotate_pose_msg_by_euler_angles(pose, 0.0, 0.0, grasp_pose[3]) # open the gripper self.robot.open_gripper() # approach to the pose self._approach(pose) # close gripper self.robot.close_gripper() # retract to clear object self._retract() def place(self, place_position): """ Place to given pose """ # Calculate pose from place position pose = get_pose(position=place_position) # approach to the pose self._approach(pose) # open the gripper self.robot.open_gripper() # Get the next grasp pose np.save(self.grasp_request, 1) # retract to clear object self._retract() def run(self): # Connect to robot self.robot.connect() # Calibrate gripper self.robot.calibrate_gripper() # Initialize grasp request and grasp available np.save(self.grasp_request, 0) np.save(self.grasp_available, 0) # Move robot to home pose print('Moving to start position...') self.robot.go_home() self.robot.open_gripper() # Get the first grasp pose np.save(self.grasp_request, 1) while not rospy.is_shutdown(): print('Waiting for grasp pose...') while not np.load( self.grasp_available) and not rospy.is_shutdown(): time.sleep(0.1) grasp_pose = np.load(self.grasp_pose) np.save(self.grasp_available, 0) # Perform pick print('Picking from ', grasp_pose) self.pick(grasp_pose) # Perform place print('Placing to ', self.place_position) self.place(self.place_position)