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, 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
# 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):
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'
except rospy.ServiceException, e:
print "Service call failed: %s" % e
return 'failed'
if hole.label == 'None':
return 'failed'
object_name = holes_to_obj[hole.label]
print object_name
height = get_place_height_for_obj(object_name)
p = hole.pose.pose.position
test_point = [grip_point[0] + p.x, p.y, grip_point[2] + height]
side = get_pickup_side(pose['name'])
conf = call_ik_solver(test_point, side=side)
if len(conf) == 0:
return 'too_far'
object_out = None
for o in objects_on_plate:
if o.label == object_name:
object_out = o
break
print object_out
userdata.object_out = backplate_object_msg_to_dict(object_out)
userdata.backplate_pose_out = object_out.place_loc
userdata.point_out = hole.pose.pose
print object_out, hole.label
return 'success'
except rospy.ServiceException, e:
print "Service call failed: %s" % e
return 'failed'
if hole.label == 'None':
return 'failed'
object_name = holes_to_obj[hole.label]
print object_name
height = get_place_height_for_obj(object_name)
p = hole.pose.pose.position
test_point = [grip_point[0] + p.x, p.y, grip_point[2] + height]
side = get_pickup_side(pose['name'])
conf = call_ik_solver(test_point, side=side)
if len(conf) == 0:
return 'too_far'
object_out = None
for o in objects_on_plate:
if o.label == object_name:
object_out = o
break
print object_out
userdata.object_out = backplate_object_msg_to_dict(object_out)
userdata.backplate_pose_out = object_out.place_loc
userdata.point_out = hole.pose.pose
print object_out, hole.label
return 'success'
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'