class pick_and_place():
def __init__(self, pick_id=0, place_id=1):
self.pick_pose = [-999]*7
self.pick_data = False
self.place_pose = [-999]*7
self.place_data = False
self.subscriber = rospy.Subscriber('ar_pose_marker', AlvarMarkers, \
self.ar_callback)
self.ctrl = ArmController(event_detector=True)
self.tf_transformer = TfTransformer(self.ctrl.tf_listener)
self.joint_move_arm_to_side('r', blocking=False)
self.joint_move_arm_to_side('l')
self.default_frame = "base_link"
self.grasper = ArghGrasp()
"""
### experimental code
def pick_up(self, whicharm='l', top_grasp = False):
rospy.loginfo("pre pickup pose")
self.open_gripper(whicharm)
first = [1.042181865195266, 0.05674577918307219, 1.7141703410128142, -1.2368941648991192, 44.01523148337816, -1.3485333030083617, 58.22465995394929]
self.ctrl.joint_movearm(whicharm, first)
start = [0.6169387251649058, 0.13840014757000843, 0.7266251135486242, 0.7131394766246717, -0.7008370665851971, 0.008851073879117963, 0.013459252242671303]
end = [0.591262509439721, -0.19429620128938668, 0.748909628486445, 0.7368007601058577, -0.6756313110616471, -0.01694214819160356, -0.01897195391107014]
self.move_cartesian_step(whicharm, start)
self.move_cartesian_step(whicharm, end, blocking=False)
self.ctrl.gripper.start_gripper_event_detector(\
whicharm, blocking=True,timeout = 5)
self.ctrl.cart_freeze_arm(whicharm)
self.close_gripper(whicharm)
curr = self.ctrl.get_cartesian_pose()[whicharm]
curr[2] += 0.05
self.move_cartesian_step(whicharm,curr)
self.constrained_move_arm_to_side(whicharm)
return True
"""
def pick_up(self, whicharm='l'):
if self.pick_data:
self.open_gripper(whicharm)
if not self.grasper.pick_up(whicharm,5, self.pick_pose):
return False
# move arm to side
rospy.loginfo("moving arm to side")
self.constrained_move_arm_to_side(whicharm)
return self.ctrl.gripper.is_grasping()[whicharm]
else:
return False
def reset_ar(self):
self.pick_data = False
self.place_data = False
def is_close(self, desired = .664, threshold=.76):
x = self.place_pose[0]
if x > threshold:
return (False, x - desired)
else:
return (True, 0)
def place(self, whicharm="l", top_grasp = True, use_offset = True):
if self.place_data:
# get place offset from cooler
if use_offset:
trans, _ = self.tf_transformer.get_transform(\
'base_link', 'ar_marker_1')
if not trans:
return False
else:
print "not finding place pose"
trans = self.place_pose[:3]
curr_pose = self.ctrl.get_cartesian_pose()[whicharm]
rot = curr_pose[3:]
#_,rot = self.ctrl.get_cartesian_pose()[whicharm]
place_pose = trans + rot
over_pose, under_pose = list(place_pose), list(place_pose)
over_pose[2] += .2
under_pose[2] -= .2
self.reset_ar()
print "%s\n%s\n%s"%(place_pose, over_pose, under_pose)
rospy.loginfo("moving arm over place pose")
result = self.move_cartesian_step(whicharm,over_pose)
rospy.loginfo("waiting a second before placing")
rospy.sleep(1) # wait a second before placing
if result:
rospy.loginfo("guarded move arm to place pose")
result = self.guarded_cartesian_move(whicharm, under_pose)
if result:
rospy.loginfo("placing can")
self.open_gripper(whicharm)
rospy.loginfo("moving arm up over place pose")
self.move_cartesian_step(whicharm, over_pose)
else:
rospy.loginfo("failed doing guarded move")
else:
rospy.loginfo("failed moving arm over")
"""
rospy.loginfo("moving arm up over place pose")
tmp = False
for i in range (5):
if tmp:
break
tmp = self.move_cartesian_step(whicharm, over_pose)
if no`t tmp:
rospy.loginfo("having difficulty moving arm up "
"over place pose")
if not tmp:
raw_input("can't find over pose")
"""
self.joint_move_arm_to_side(whicharm)
rospy.loginfo("place result succes is: %s " % result)
return result
else:
rospy.loginfo("no place data")
return False
def ar_callback(self, data):
for markers in data.markers:
try:
px = markers.pose.pose.position.x
py = markers.pose.pose.position.y
pz = markers.pose.pose.position.z
ox = markers.pose.pose.orientation.x
oy = markers.pose.pose.orientation.y
oz = markers.pose.pose.orientation.z
ow = markers.pose.pose.orientation.w
if markers.id == 0: # pick
self.pick_pose = [px,py,pz,ox,oy,oz,ow]
self.pick_data = True
if markers.id == 1: # place
self.place_pose = [px, py, pz, ox, oy, oz, ow]
self.place_data = True
except:
pass
#open the gripper
def open_gripper(self, whicharm):
self.ctrl.open_gripper(whicharm)
#close the gripper
def close_gripper(self, whicharm):
self.ctrl.close_gripper(whicharm)
def constrained_move_arm_to_side(self, whicharm):
location={'l': [0.05, 0.65, .7], 'r': [0.05, -0.65, .7]}
poses = self.ctrl.get_cartesian_pose()
rot = poses[whicharm][3:]
side_pose = location[whicharm] + rot
result = self.move_cartesian_step(whicharm, side_pose, blocking=True)
if not result:
rospy.loginfo("cannot constrain joint moving to side")
self.joint_move_arm_to_side(whicharm)
def joint_move_arm_to_side(self, whicharm, blocking = 1):
self.arm_to_side_angles = \
{'r': [-2.02, 0.014, -1.73, -1.97, 10.91, -1.42, 14.17],
'l': [2.02, 0.014, 1.73, -1.97, -10.91, -1.42, 14.17]}
self.ctrl.joint_movearm(whicharm,self.arm_to_side_angles[whicharm],\
blocking=blocking)
def guarded_cartesian_move(self, whicharm, pose, frame="base_link",\
move_duration = 15):
start_pose = self.ctrl.get_cartesian_pose()[whicharm][:3]
self.move_cartesian_step(whicharm, pose, move_duration=7.0,\
frame_id=frame, blocking=False)
self.ctrl.gripper.start_gripper_event_detector(\
whicharm, blocking=True,timeout=move_duration)
self.ctrl.cancel_goal(whicharm)
# did arm even move?
curr_pose = self.ctrl.get_cartesian_pose()[whicharm][:3]
diff = np.linalg.norm(np.array(start_pose) - np.array(curr_pose))
not_at_start = diff > 0.1
return not_at_start
#move to a Cartesian pose goal
def move_cartesian_step(self, whicharm, pose, frame_id="base_link",\
move_duration=5.0, ik_timeout=5.0, blocking=True):
result = self.ctrl.cart_movearm(whicharm, pose, frame_id, \
move_duration, ik_timeout, blocking)
return result
class Primitives:
block_size = 1e-3 * 13.0 # mm
W,H = 10.0, 20.0 # tetris grid units
z_above = 0.07
z_down = 0.03
sqr2 = np.sqrt(2.0)/2.0
sqr7 = np.sqrt(7.0)/4.0
vert = [-.5, .5, .5, .5]
horz = [0, sqr2, 0, sqr2]
#rot_r = [-sqr7, -.25, sqr7, -.25]
rot_r = [-.25, sqr7, 0.25, sqr7]
rot_l = [sqr7, -.25, -sqr7, -.25]
init_pose = {
#'r': [-0.73, -0.17, -1.60, -1.46, -32.69, -1.31, -16.94],
#'l': [ 0.71, -0.04, 1.56, -1.46, -4.72, -1.36, 3.86]}
'l': [ 0.95, 0.0, np.pi/2., -np.pi/2., -np.pi*1.5, -np.pi/2.0, np.pi],
'r': [ -0.7, 0.0, -np.pi/2., -np.pi/2., -20.424894658897493, -np.pi/2.0, np.pi]}
min_time = 1.0
max_time = 5.0
frame = 'tetris'
def __init__(self, whicharm='l'):
self.arm = ArmController()
self.objects = None
self.obj_sub = rospy.Subscriber("/table_publisher/object_poses", PoseArray, self.objCb)
self.arm.command_torso(0.26)
self.whicharm = whicharm
self.move_arm_to_side()
def move_arm_to_side(self):
self.arm.joint_movearm(self.whicharm, self.init_pose[self.whicharm],
3, True)
def lift_arm(self, rating=1.0):
rospy.sleep(0.1)
curr_pos = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
curr_pos[2] = self.z_above *rating
goal =[ self.stiff_pose(curr_pos, 0.1) + [0.25*self.min_time*rating],
self.stiff_pose(curr_pos, 0.5) + [0.5*self.min_time*rating],
self.stiff_pose(curr_pos, 1.0) + [self.min_time*rating] ]
self.arm.cart_movearm(self.whicharm, goal , self.frame, True)
#raw_input("done lift")
def stiff_pose(self, pose, rating = 0.2):
pose_force = int(100*rating)
torque_force = int(50*rating)
return list(pose) + [pose_force]*3 + [torque_force]*3 + [False]*6
def distance_between_two_pts(self, p1,p2):
pts = zip(p1,p2)
dx = [(x1 - x2)**2 for (x1,x2) in pts]
return sum(dx)
def clip_time(self, t):
if t < self.min_time:
return self.min_time
elif t > self.max_time:
return self.max_time
else:
return t
def approach_pose(self, pose):
curr_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
time = self.clip_time(15*self.distance_between_two_pts(curr_pose, pose))
curr_pose[2] = self.z_above
middle_pose = list(pose)
middle_pose[2] = self.z_above
traj = [
self.stiff_pose(curr_pose, 0.1) + [time*.1],
self.stiff_pose(curr_pose, 1.0) + [time*.2],
self.stiff_pose(middle_pose, 1.5) + [time*.8],
self.free_push(pose, 1.5) + [time]
]
self.arm.cart_movearm(self.whicharm, traj, self.frame, True)
rospy.sleep(0.1)
#final_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
#dist = self.distance_between_two_pts(pose, final_pose)
def do_force_control(self, cmd, timeout=4.0):
self.approach_pose(cmd[:7])
rospy.sleep(0.1)
curr_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
ros_timeout = rospy.Time.now() + rospy.Duration(timeout)
self.arm.cart_movearm(self.whicharm, [cmd + [timeout]], self.frame, False)
rospy.sleep(0.2)
while rospy.Time.now() < ros_timeout and not rospy.is_shutdown():
rospy.sleep(0.1)
if not self.arm.is_moving(self.whicharm):
self.arm.cancel_goal(self.whicharm)
break
"""
final_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
if self.distance_between_two_pts(curr_pose, final_pose) < self.block_size:
ros_timeout = rospy.Time.now() + rospy.Duration(timeout)
self.arm.cart_movearm(self.whicharm, [cmd + [timeout]], self.frame, False)
rospy.sleep(1.0)
while rospy.Time.now() < ros_timeout:
rospy.sleep(0.1)
if not self.arm.is_moving(self.whicharm):
self.arm.cancel_goal(self.whicharm)
break
"""
self.lift_arm()
def free_space_push_down(self, pos1, pos2):
pose1 = list(pos1) + self.horz
pose2 = list(pos2) + self.horz
time = self.clip_time(15*self.distance_between_two_pts(pose1,pose2))
self.approach_pose(pose1)
traj = [
self.free_push(pose1) + [time*.1],
self.free_push(pose2) + [time]
]
self.arm.cart_movearm(self.whicharm, traj, self.frame, True)
self.lift_arm(1.)
def free_space_push_right(self, pos1, pos2):
pose1 = list(pos1) + self.vert
pose2 = list(pos2) + self.vert
time = self.clip_time(15*self.distance_between_two_pts(pose1,pose2))
self.approach_pose(pose1)
traj = [
self.free_push(pose1) + [time*.1],
self.free_push(pose2) + [time]
]
self.arm.cart_movearm(self.whicharm, traj, self.frame, True)
self.lift_arm(1.)
def free_push(self, pose, rating=1.0): # position controlled
fx,fy,fz = [300*rating]*3 # max stiffness
ox,oy,oz = [10*rating]*3
fz = -5*rating # force in downard direction
states = [False]*6
states[2] = True # use force control for Z
return list(pose) + [fx,fy,fz,ox,oy,oz] + states
def right_contact_slide_down(self, pos, rating=1.0):
pose = list(pos) + self.rot_r
#use force control
fx = -7*rating
fy = -3 * rating
fz = -2* rating
#keep gripper rotated
ox,oy,oz = [30]*3
states =[True, True, True] + [False]*3
cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states
self.do_force_control(cmd)
def left_contact_slide_down(self, pos, rating=1.0):
pose = list(pos) + self.rot_l
#use force control
fx = -7*rating
fy = 2*rating
fz = -2*rating
#keep gripper rotated
ox,oy,oz = [30]*3
states =[True, True, True] + [False]*3
cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states
self.do_force_control(cmd)
def right_contact_slide_right(self, pos, rating=1.0):
pose = list(pos) + self.rot_r
#use force control
fx = -2*rating
fy = -5*rating
fz = -2*rating
#keep gripper rotated
ox,oy,oz = [30]*3
states =[True, True, True] + [False]*3
cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states
self.do_force_control(cmd)
def push_down(self, pos, rating=1.0):
pose = list(pos) + self.horz
#use force control
fx = -7*rating
fy = 300*rating
fz = -3*rating
#keep gripper rotated
ox,oy,oz = [30]*3
states =[True, False, True] + [False]*3
cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states
self.do_force_control(cmd)
def push_right(self, pos, rating=1.0):
pose = list(pos) + self.vert
#use force control
fx = 400*rating
fy = -5 *rating
fz = -3*rating
#keep gripper rotated
ox,oy,oz = [30]*3
states =[False, True, True] + [False]*3
cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states
self.do_force_control(cmd)
def push_left(self, pos, rating=1.0):
pose = list(pos) + self.vert
#use force control
fx = 300*rating
fy = 5 *rating
fz = -3*rating
#keep gripper rotated
ox,oy,oz = [30]*3
states =[False, True, True] + [False]*3
cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states
self.do_force_control(cmd)
def convert_xy(self, (x,y) ):
return (x*self.block_size, y*self.block_size, self.z_down)