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)