def execute(self, ud):
position = ud.tb_pose_in.position
p = np.array([position.x, position.y])
dist = 0.28 + 0.07
norm = p / np.linalg.norm(p)
p -= dist * norm
point = [p[0], -p[1], 0]
conf = call_ik_solver(point, side='front')
while len(conf) == 0:
p -= 0.01 * norm
point = [p[0], -p[1], 0]
conf = call_ik_solver(point, side='front')
print p
print conf
conf = np.array(conf)
conf[0] += 0.2
conf[3] -= math.pi / 2.
conf[4] = joints['arm_joint_5']['straight']
go([conf], blocking=True)
rospy.sleep(1.0)
gripper_close(False)
return 'done'
def execute(self, userdata):
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
rotate = rotate_only(side=side, turned=False)
go([rotate])
return 'done'
def execute(self, userdata):
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
rotate = rotate_only(side=side, turned=False)
go([rotate])
return 'done'
def execute(self, ud):
position = ud.tb_pose_in.position
p = np.array([position.x, position.y])
dist = 0.28 + 0.07
norm = p / np.linalg.norm(p)
p -= dist * norm
point = [p[0], -p[1], 0]
conf = call_ik_solver(point, side='front')
while len(conf) == 0:
p -= 0.01 * norm
point = [p[0], -p[1], 0]
conf = call_ik_solver(point, side='front')
print p
print conf
conf = np.array(conf)
conf[0] += 0.2
conf[3] -= math.pi / 2.
conf[4] = joints['arm_joint_5']['straight']
go([conf], blocking=True)
rospy.sleep(1.0)
gripper_close(False)
return 'done'
def execute(self, userdata):
# locations = rosparam.get('locations')
# pose = locations[0]
#pre_grip[0] = rotate[0]
go([cam_test_drop])
gripper_close(False)
return 'done'
def execute(self, userdata):
# locations = rosparam.get('locations')
# pose = locations[0]
#pre_grip[0] = rotate[0]
go([cam_test_drop])
gripper_close(False)
return 'done'
def execute(self, userdata):
# locations = rosparam.get('locations')
# pose = locations[0]
up = [x for x in cam_test]
up[0] = configuration[0]
go([up])
return 'done'
def execute(self, userdata):
arm_up = [
configuration[0], joints['arm_joint_2']['straight'],
joints['arm_joint_3']['straight'],
joints['arm_joint_4']['straight'], configuration[4]
]
go([arm_up], blocking=self.blocking)
return 'done'
def execute(self, userdata):
# locations = rosparam.get('locations')
# pose = locations[0]
up = [x for x in cam_test]
up[0] = configuration[0]
go([up])
return 'done'
def execute(self, userdata):
side = 'front'
rotate = rotate_only(side)
conf = [x for x in pre_grip]
conf[0] = rotate[0] + 0.2
conf[3] -= 0.3
conf[1] = joints['arm_joint_2']['straight']
# gripperClose(False)
# rotate,
go([conf], blocking=self.blocking)
return 'success'
def execute(self, userdata):
side = 'front'
rotate = rotate_only(side)
conf = [x for x in pre_grip]
conf[0] = rotate[0] + 0.2
conf[3] -= 0.3
conf[1] = joints['arm_joint_2']['straight']
# gripperClose(False)
# rotate,
go([conf], blocking=self.blocking)
return 'success'
def execute(self, userdata):
pose = userdata.pose_in
# position = rospy.get_param(pose)
# print 'go', pose
side = get_pickup_side(pose['name'])
#print 'go'
rotate = rotate_only(side)
pre_grip[0] = rotate[0]
# gripperClose(False)
# rotate,
go([pre_grip], blocking=self.blocking)
return 'success'
def execute(self, userdata):
pose = userdata.pose_in
# position = rospy.get_param(pose)
# print 'go', pose
side = get_pickup_side(pose['name'])
#print 'go'
rotate = rotate_only(side)
pre_grip[0] = rotate[0]
# gripperClose(False)
# rotate,
go([pre_grip], blocking=self.blocking)
return 'success'
def execute(self, userdata):
# side = position['side']
rotate = rotate_only('left', turned=False)
arm_up = [
rotate[0], joints['arm_joint_2']['straight'],
joints['arm_joint_3']['straight'],
joints['arm_joint_4']['straight'], rotate[4]
]
tuck = [
rotate[0], joints['arm_joint_2']['min'],
joints['arm_joint_3']['max'], joints['arm_joint_4']['min'],
rotate[4]
]
# print arm_up, tuck
#return 'success'
if go([rotate, arm_up, tuck]):
self.counter = 0
return 'success'
else:
if self.counter < MAX_TRIES:
self.counter += 1
return 'not_reached'
else:
self.counter = 0
return 'failed'
def execute(self, userdata):
rospy.sleep(4)
gripper_close(True)
tucked = [x for x in configuration]
# tucked[0] = configuration[0]
tucked[1] = joints['arm_joint_2']['min']
tucked[2] = joints['arm_joint_3']['max']
tucked[3] = joints['arm_joint_4']['min']
# tucked[4] = configuration[4]
tucked_for_drive = rotate_only(side='left')
#tucked[0] = conf[0]
tucked_for_drive[1] = joints['arm_joint_2']['min']
tucked_for_drive[2] = joints['arm_joint_3']['max']
tucked_for_drive[3] = joints['arm_joint_4']['min']
tucked_for_drive[4] = joints['arm_joint_5']['straight']
go([tucked, tucked_for_drive])
return 'end'
def execute(self, userdata):
pose = userdata.pose_in
position = rospy.get_param(pose)
side = position['side']
rotate = rotate_only(side, turned=True)
pre_kinect[0] = rotate[0]
userdata.pose_out = pose
if go([rotate, pre_kinect]):
return 'success'
else:
return 'failed'
def execute(self, userdata):
pose = userdata.pose_in
position = rospy.get_param(pose)
side = position['side']
rotate = rotate_only(side, turned=True)
pre_kinect[0] = rotate[0]
userdata.pose_out = pose
if go([rotate, pre_kinect]):
return 'success'
else:
return 'failed'
def execute(self, userdata):
rospy.sleep(4)
gripper_close(True)
tucked = [x for x in configuration]
# tucked[0] = configuration[0]
tucked[1] = joints['arm_joint_2']['min']
tucked[2] = joints['arm_joint_3']['max']
tucked[3] = joints['arm_joint_4']['min']
# tucked[4] = configuration[4]
tucked_for_drive = rotate_only(side='left')
#tucked[0] = conf[0]
tucked_for_drive[1] = joints['arm_joint_2']['min']
tucked_for_drive[2] = joints['arm_joint_3']['max']
tucked_for_drive[3] = joints['arm_joint_4']['min']
tucked_for_drive[4] = joints['arm_joint_5']['straight']
go([tucked, tucked_for_drive])
return 'end'
def execute(self, userdata):
# locations = rosparam.get('locations')
# pose = locations[0]
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
rotate = rotate_only(side)
#pre_grip[0] = rotate[0]
#up = [x for x in cam_test]
#up[0] = configuration[0]
if go([rotate]):
return 'success'
else:
return 'failed'
def execute(self, userdata):
# locations = rosparam.get('locations')
# pose = locations[0]
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
rotate = rotate_only(side)
#pre_grip[0] = rotate[0]
#up = [x for x in cam_test]
#up[0] = configuration[0]
if go([rotate]):
return 'success'
else:
return 'failed'
def execute(self, userdata):
confs = []
if not self.no_rotate:
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
# print joints
tucked = rotate_only(side='left')
#tucked[0] = configuration[0]
#tucked[1] = joints['arm_joint_2']['min']
#tucked[2] = joints['arm_joint_3']['max']
#tucked[3] = joints['arm_joint_4']['min']
#tucked[4] = configuration[4]
confs.append(tucked)
tucked_for_drive = rotate_only(side='left')
# tucked[0] = conf[0]
tucked_for_drive[1] = joints['arm_joint_2']['min']
tucked_for_drive[2] = joints['arm_joint_3']['max']
tucked_for_drive[3] = joints['arm_joint_4']['min']
tucked_for_drive[4] = joints['arm_joint_5']['straight']
confs.append(tucked_for_drive)
go(confs, blocking=self.blocking)
return 'done'
def execute(self, userdata):
confs = []
if not self.no_rotate:
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
# print joints
tucked = rotate_only(side='left')
#tucked[0] = configuration[0]
#tucked[1] = joints['arm_joint_2']['min']
#tucked[2] = joints['arm_joint_3']['max']
#tucked[3] = joints['arm_joint_4']['min']
#tucked[4] = configuration[4]
confs.append(tucked)
tucked_for_drive = rotate_only(side='left')
# tucked[0] = conf[0]
tucked_for_drive[1] = joints['arm_joint_2']['min']
tucked_for_drive[2] = joints['arm_joint_3']['max']
tucked_for_drive[3] = joints['arm_joint_4']['min']
tucked_for_drive[4] = joints['arm_joint_5']['straight']
confs.append(tucked_for_drive)
go(confs, blocking=self.blocking)
return 'done'
def execute(self, userdata):
pose = userdata.pose_in
# position = rospy.get_param(pose)
userdata.pose_out = pose
side = get_pickup_side(pose['name'])
rotate = rotate_only(side)
# pre_grip[0] = rotate[0]
gripper_close(False)
new_grip_point = [x for x in grip_point]
new_grip_point[0] += 0.05
new_grip_point[2] += 0.015
conf = np.array(call_ik_solver(new_grip_point, side=side))
conf[4] = get_angle(conf[4], math.pi / 2)
if go([rotate, conf]):
return 'success'
else:
return 'failed'
def execute(self, userdata):
pose = userdata.pose_in
# position = rospy.get_param(pose)
userdata.pose_out = pose
side = get_pickup_side(pose['name'])
rotate = rotate_only(side)
# pre_grip[0] = rotate[0]
gripper_close(False)
new_grip_point = [x for x in grip_point]
new_grip_point[0] += 0.05
new_grip_point[2] += 0.015
conf = np.array(call_ik_solver(new_grip_point, side=side))
conf[4] = get_angle(conf[4], math.pi / 2)
if go([rotate, conf]):
return 'success'
else:
return 'failed'
def execute(self, userdata):
# side = position['side']
rotate = rotate_only('left', turned=False)
arm_up = [rotate[0], joints['arm_joint_2']['straight'], joints['arm_joint_3']['straight'],
joints['arm_joint_4']['straight'], rotate[4]]
tuck = [rotate[0], joints['arm_joint_2']['min'], joints['arm_joint_3']['max'], joints['arm_joint_4']['min'],
rotate[4]]
# print arm_up, tuck
#return 'success'
if go([rotate, arm_up, tuck]):
self.counter = 0
return 'success'
else:
if self.counter < MAX_TRIES:
self.counter += 1
return 'not_reached'
else:
self.counter = 0
return 'failed'
def execute(self, userdata):
obj = userdata.object_in
# offset detected by object detection
offset = userdata.point_in
pose = userdata.pose_in
# to which side
side = get_pickup_side(pose['name'])
quat = [
offset.orientation.x, offset.orientation.y, offset.orientation.z,
offset.orientation.w
]
r, p, y = tf.transformations.euler_from_quaternion(quat)
new_grip_point = [x for x in grip_point]
new_grip_point[0] += offset.position.x
new_grip_point[1] -= offset.position.y
new_grip_point[2] += get_pick_height_for_obj(obj['label'])
publish_status('I am grasping object ' + str(obj['name']) + '.')
## TODO check HEIGHT FOR HIGH OBJECTS
if NO_ARM:
return 'success'
conf = None
try:
conf = call_ik_solver(new_grip_point, side=side)
except:
print "got no config from IK Solver"
if len(conf) == 0:
return 'too_far'
else:
conf = np.array(conf)
grip_angle = get_angle(conf[4], y)
straight_angle = conf[4]
conf[4] = grip_angle
height = new_grip_point[2]
new_grip_point[2] += PRE_GRIP_HEIGHT
confs = []
rotate = rotate_only(side=side)
confs.append(rotate)
first = None
gripX = new_grip_point[0]
gripY = new_grip_point[1]
while new_grip_point[2] > height:
new_grip_point[2] -= STEP_SIZE
new_grip_point[2] = max(height, new_grip_point[2])
new_grip_point[0] = gripX
new_grip_point[1] = gripY
conf2 = []
while len(conf2) == 0:
try:
print 'get conf', new_grip_point
conf2 = call_ik_solver(new_grip_point, side=side)
print conf2
except:
#new_grip_point[0] -= 0.01 #get closer to solve pre-grip
print 'got not config'
return 'too_far'
if len(conf2) > 0:
conf2 = np.array(conf2)
print conf2
new_grip_point[0] -= 0.005 #get closer to solve pre-grip
conf2[4] = conf[4]
if first is None:
first = [x for x in conf2]
confs.append(conf2)
gripper_close(False)
confs.append(conf)
if not go(confs):
return 'failed'
if userdata.object_in['label'] == 'M20_100_h':
obj = userdata.object_in
### TODO set correct rotation
obj['rotation'] = -y - (grip_angle - straight_angle)
userdata.object_out = obj
print "rotation is", obj[
'rotation'], y, grip_angle - straight_angle
gripper_close(True, heavy=True)
else:
gripper_close(True)
if not go([first]):
return 'failed_after_grip'
return 'success'
def execute(self, userdate):
up = [x for x in cam_test]
up[0] = configuration[0]
go([cam_test])
return 'done'
def execute(self, userdate):
up = [x for x in cam_test]
up[0] = configuration[0]
go([cam_test])
return 'done'
def execute(self, userdata):
arm_up = [configuration[0], joints['arm_joint_2']['straight'], joints['arm_joint_3']['straight'],
joints['arm_joint_4']['straight'], configuration[4]]
go([arm_up], blocking=self.blocking)
return 'done'
def execute(self, userdata):
obj = userdata.object_in
# offset detected by object detection
offset = userdata.point_in
pose = userdata.pose_in
# to which side
side = get_pickup_side(pose['name'])
quat = [offset.orientation.x, offset.orientation.y, offset.orientation.z, offset.orientation.w]
r, p, y = tf.transformations.euler_from_quaternion(quat)
new_grip_point = [x for x in grip_point]
new_grip_point[0] += offset.position.x
new_grip_point[1] -= offset.position.y
new_grip_point[2] += get_pick_height_for_obj(obj['label'])
publish_status('I am grasping object ' + str(obj['name']) + '.')
## TODO check HEIGHT FOR HIGH OBJECTS
if NO_ARM:
return 'success'
conf = None
try:
conf = call_ik_solver(new_grip_point, side=side)
except:
print "got no config from IK Solver"
if len(conf) == 0:
return 'too_far'
else:
conf = np.array(conf)
grip_angle = get_angle(conf[4], y)
straight_angle = conf[4]
conf[4] = grip_angle
height = new_grip_point[2]
new_grip_point[2] += PRE_GRIP_HEIGHT
confs = []
rotate = rotate_only(side=side)
confs.append(rotate)
first = None
gripX = new_grip_point[0]
gripY = new_grip_point[1]
while new_grip_point[2] > height:
new_grip_point[2] -= STEP_SIZE
new_grip_point[2] = max(height, new_grip_point[2])
new_grip_point[0] = gripX
new_grip_point[1] = gripY
conf2 = []
while len(conf2) == 0:
try:
print 'get conf', new_grip_point
conf2 = call_ik_solver(new_grip_point, side=side)
print conf2
except:
#new_grip_point[0] -= 0.01 #get closer to solve pre-grip
print 'got not config'
return 'too_far'
if len(conf2) > 0:
conf2 = np.array(conf2)
print conf2
new_grip_point[0] -= 0.005 #get closer to solve pre-grip
conf2[4] = conf[4]
if first is None:
first = [x for x in conf2]
confs.append(conf2)
gripper_close(False)
confs.append(conf)
if not go(confs):
return 'failed'
if userdata.object_in['label'] == 'M20_100_h':
obj = userdata.object_in
### TODO set correct rotation
obj['rotation'] = -y - (grip_angle - straight_angle)
userdata.object_out = obj
print "rotation is", obj['rotation'], y, grip_angle - straight_angle
gripper_close(True, heavy=True)
else:
gripper_close(True)
if not go([first]):
return 'failed_after_grip'
return 'success'
def execute(self, userdata):
# offset detected by object detection
offset = userdata.point_in
pose = userdata.pose_in
obj = userdata.object_in
side = get_pickup_side(pose['name'])
# to which side
publish_status('I am placing the object <br> on the service area.')
quat = [offset.orientation.x, offset.orientation.y, offset.orientation.z, offset.orientation.w]
r, p, y = tf.transformations.euler_from_quaternion(quat)
new_grip_point = [x for x in grip_point]
new_grip_point[0] += offset.position.x
new_grip_point[1] -= offset.position.y
new_grip_point[2] += get_place_height_for_obj(obj['label'])
if NO_ARM:
return 'success'
conf = None
try:
conf = call_ik_solver(new_grip_point, side=side)
except:
print "got no config from IK Solver"
if len(conf) == 0:
return 'too_far'
else:
conf = np.array(conf)
straight = conf[4]
grip_angle = get_angle(conf[4], y)
conf[4] = grip_angle
new_grip_point[2] += PRE_PLACE_HEIGHT
rotate = rotate_only(side=side)
gripX = new_grip_point[0]
gripY = new_grip_point[1]
new_grip_point[0] = gripX
new_grip_point[1] = gripY
conf2 = []
while len(conf2) == 0:
try:
print 'get conf', new_grip_point
conf2 = call_ik_solver(new_grip_point, side=side)
print conf2
except:
# new_grip_point[0] -= 0.01 #get closer to solve pre-grip
print 'got not config'
return 'too_far'
if len(conf2) > 0:
conf2 = np.array(conf2)
new_grip_point[0] -= 0.005 #get closer to solve pre-grip
conf2[4] = conf[4]
go([rotate])
print obj
if obj['label'] == 'M20_100_h' and obj['type'] == 'PPT':
m20_rot = obj['rotation']
yaw = self.get_rotation(y, straight, m20_rot)
if yaw is None:
print "rotate screw!"
yaw = self.get_rotation(y, straight, math.pi - abs(m20_rot))
temp_y = get_angle(straight, y - math.pi / 2.)
scnd_tmp = temp_y + math.pi
if scnd_tmp > joints['arm_joint_5']['max']:
scnd_tmp -= 2. * math.pi
conf[4] = temp_y
conf2[4] = temp_y
go([conf2, conf])
gripper_close(False)
go([conf2])
conf[4] = scnd_tmp
conf2[4] = scnd_tmp
go([conf2, conf])
gripper_close(True, heavy=True)
go([conf2])
conf[4] = yaw
conf2[4] = yaw
if not go([conf2, conf]):
return 'failed'
gripper_close(False)
#rotate??
if False:
rotate_wiggle = [x for x in conf]
rotate_wiggle[4] -= 0.1
rotate_wiggle2 = [x for x in conf]
rotate_wiggle2[4] += 0.1
if not go([rotate_wiggle, rotate_wiggle2, conf]):
return 'failed'
if not go([conf2]):
return 'failed_after_place'
return 'success'
def execute(self, userdata):
# offset detected by object detection
offset = userdata.point_in
pose = userdata.pose_in
obj = userdata.object_in
side = get_pickup_side(pose['name'])
# to which side
publish_status('I am placing the object <br> on the service area.')
quat = [
offset.orientation.x, offset.orientation.y, offset.orientation.z,
offset.orientation.w
]
r, p, y = tf.transformations.euler_from_quaternion(quat)
new_grip_point = [x for x in grip_point]
new_grip_point[0] += offset.position.x
new_grip_point[1] -= offset.position.y
new_grip_point[2] += get_place_height_for_obj(obj['label'])
if NO_ARM:
return 'success'
conf = None
try:
conf = call_ik_solver(new_grip_point, side=side)
except:
print "got no config from IK Solver"
if len(conf) == 0:
return 'too_far'
else:
conf = np.array(conf)
straight = conf[4]
grip_angle = get_angle(conf[4], y)
conf[4] = grip_angle
new_grip_point[2] += PRE_PLACE_HEIGHT
rotate = rotate_only(side=side)
gripX = new_grip_point[0]
gripY = new_grip_point[1]
new_grip_point[0] = gripX
new_grip_point[1] = gripY
conf2 = []
while len(conf2) == 0:
try:
print 'get conf', new_grip_point
conf2 = call_ik_solver(new_grip_point, side=side)
print conf2
except:
# new_grip_point[0] -= 0.01 #get closer to solve pre-grip
print 'got not config'
return 'too_far'
if len(conf2) > 0:
conf2 = np.array(conf2)
new_grip_point[0] -= 0.005 #get closer to solve pre-grip
conf2[4] = conf[4]
go([rotate])
print obj
if obj['label'] == 'M20_100_h' and obj['type'] == 'PPT':
m20_rot = obj['rotation']
yaw = self.get_rotation(y, straight, m20_rot)
if yaw is None:
print "rotate screw!"
yaw = self.get_rotation(y, straight, math.pi - abs(m20_rot))
temp_y = get_angle(straight, y - math.pi / 2.)
scnd_tmp = temp_y + math.pi
if scnd_tmp > joints['arm_joint_5']['max']:
scnd_tmp -= 2. * math.pi
conf[4] = temp_y
conf2[4] = temp_y
go([conf2, conf])
gripper_close(False)
go([conf2])
conf[4] = scnd_tmp
conf2[4] = scnd_tmp
go([conf2, conf])
gripper_close(True, heavy=True)
go([conf2])
conf[4] = yaw
conf2[4] = yaw
if not go([conf2, conf]):
return 'failed'
gripper_close(False)
#rotate??
if False:
rotate_wiggle = [x for x in conf]
rotate_wiggle[4] -= 0.1
rotate_wiggle2 = [x for x in conf]
rotate_wiggle2[4] += 0.1
if not go([rotate_wiggle, rotate_wiggle2, conf]):
return 'failed'
if not go([conf2]):
return 'failed_after_place'
return 'success'
