def __init__(self):
        init()  # initialize yarp and ros

        self.table_height = 0.855

        self.arm = HandleArm("lwr", "right", "/tmRightArm")
        self.larm = HandleArm("lwr", "left", "/tmLeftArm")
        # tool_fr = list_to_frame(default_hand_tool)
        # self.arm.setTool(tool_fr)
        # self.arm.removeAllObstaclesBlind()

        config_file = "config-lwr-right.py"
        self.hb = HandleBridge(config_file, "tmdBridge")
        self.hbl = HandleBridge("config-lwr-left.py", "tmdBridge")
        self.hjc = HandleJController(config_file, "tmJC")
        self.robsim = RobotSimulator(config_file)

        self.nav = HandleNavP()
        self.hlo = HandleLO()
        self.hptu = HandlePTU()
        self.hcop = HandleCOP()
        self.rarm_action = HandleArmHandServer("right")
        self.larm_action = HandleArmHandServer("left")
        self.rgrasper = HandleGrasper("/tm_grasper_right", "/grasper0")
        self.lgrasper = HandleGrasper("/tm_grasper_left", "/grasper1")
        self.lweight = HandleWeight("/tm_weight_left", "left")
        self.rweight = HandleWeight("/tm_weight_right", "right")

        self.normal_vel = 29.9 * pi / 180.0  # Values from the armhand server
        self.hb.set_max_vel(self.normal_vel)

        self.state_pub = rospy.Publisher("/calib_state", Int8)

        # config area
        self.single_movement_timeout = 6.0
class ToolMover(object):
    def __init__(self):
        init()  # initialize yarp and ros

        self.table_height = 0.855

        self.arm = HandleArm("lwr", "right", "/tmRightArm")
        self.larm = HandleArm("lwr", "left", "/tmLeftArm")
        # tool_fr = list_to_frame(default_hand_tool)
        # self.arm.setTool(tool_fr)
        # self.arm.removeAllObstaclesBlind()

        config_file = "config-lwr-right.py"
        self.hb = HandleBridge(config_file, "tmdBridge")
        self.hbl = HandleBridge("config-lwr-left.py", "tmdBridge")
        self.hjc = HandleJController(config_file, "tmJC")
        self.robsim = RobotSimulator(config_file)

        self.nav = HandleNavP()
        self.hlo = HandleLO()
        self.hptu = HandlePTU()
        self.hcop = HandleCOP()
        self.rarm_action = HandleArmHandServer("right")
        self.larm_action = HandleArmHandServer("left")
        self.rgrasper = HandleGrasper("/tm_grasper_right", "/grasper0")
        self.lgrasper = HandleGrasper("/tm_grasper_left", "/grasper1")
        self.lweight = HandleWeight("/tm_weight_left", "left")
        self.rweight = HandleWeight("/tm_weight_right", "right")

        self.normal_vel = 29.9 * pi / 180.0  # Values from the armhand server
        self.hb.set_max_vel(self.normal_vel)

        self.state_pub = rospy.Publisher("/calib_state", Int8)

        # config area
        self.single_movement_timeout = 6.0

    def send_state(self, state):
        self.state_pub.publish(Int8(state))

    def arm_speed(self, vel, vf_vel):
        self.hb.set_max_vel(vel)
        self.hb.set_vf_max_vel(vf_vel)

    def set_arm_speed_fast(self):
        print ("arm_speed: fast")
        self.arm_speed(39.0 * pi / 180.0, 0.39)

    def set_arm_speed_normal(self):
        print ("arm_speed: normal")
        self.arm_speed(20.0 * pi / 180.0, 0.20)

    def set_arm_speed_slow(self):
        print ("arm_speed: slow")
        self.arm_speed(20.0 * pi / 180.0, 0.10)

    def sleep(self, length):
        print "About to sleep: %4.2f" % (length)
        time.sleep(length)

    def arm_joint_distance(self, joints):
        """return the sum of the distances of all the joint angles"""
        joints_now = self.arm.getActualJoints()
        diffv = self.vector_distance(joints_now, joints)

        diff = 0.0

        for i in diffv:
            diff += abs(i)

        return diff

    def vector_distance(self, v1, v2):
        "return difference vector v1 - v2"
        if len(v1) != len(v2):
            raise Exception("vectors should have same length")
        c = []

        for i in range(len(v1)):
            c.append(v1[i] - v2[i])

        return c

    def find_toolhandle(self):
        sensor_pose = self.hlo.getLOid("/openni_rgb_optical_frame")[0]
        answer = self.hcop.searchObjects("SpatulaHandle", 1, sensor_pose, 0)  # 0 is locate

        if answer != []:

            spatula_loid = answer[0].position
            fr = self.hlo.getF(spatula_loid, "/base_link")

            print "SpatulaHandle found at %4.2f %4.2f %4.2f" % (fr.p[0], fr.p[1], fr.p[2])
            # Height should be 0.92 - 1.01

            fr.p[2] = 1.02  # 0.976
            print "Forcing height to %4.2f" % (fr.p[2])
            return fr
        else:
            return None

    def find_lines(self, num=100):
        watch_spatula_map = (-3.4357326328702631, 1.2874688908691974, 0.26729483850908814)

        intermediate = [1.651159, 0.568041, -0.509002, 1.592075, -1.150115, -0.228047, -0.480893]
        watching_spatula = [0.097839, -0.915356, 0.360731, 1.941177, -1.786561, 0.469069, -0.475895]

        joints = self.arm.getActualJoints()
        eps = 0.1
        if self.arm_joint_distance(watching_spatula) > eps:
            self.rarm_action.joint_pose("open")
            self.hjc.setJointsBlocking(intermediate, self.hb, 5.0)
        self.nav.drive_map_pose(watch_spatula_map, self.hlo)
        self.hptu.look_at(self.hlo.getLOid("/right_arm_hand_link")[0], True)
        self.hjc.setJointsBlocking(watching_spatula, self.hb, 5.0)

        self.hptu.look_at(self.hlo.getLOid("/right_arm_hand_link")[0], True)
        self.hjc.setJointsBlocking(watching_spatula, self.hb, 5.0)

        self.sleep(2)

        self.find_lines_bare(num)

    def find_lines_bare(self, num=100, cov_val=0.06):
        right_arm = self.hlo.getLOid("/right_arm_hand_link")[0]
        search_space = self.hlo.updateFrame(approx_tool, right_arm, 0, "", 0, self.hlo.diagonalCov([cov_val] * 6))

        self.hptu.look_at(search_space, False)
        answer = self.hcop.searchObjects("Line", num, search_space, 0)  # 0 is locate

        print "found %d lines" % len(answer)
        return answer

    def find_cluster(self, num=5):
        search = self.hlo.getLOid("/openni_rgb_optical_frame")[0]

        answer = self.hcop.searchObjects("Cluster", num, search, 0)  # 0 is locate

        print "found %d Cluster" % len(answer)
        return answer

    def find_obj(self, idobj, num=5):
        search = self.hlo.getLOid("/openni_rgb_optical_frame")[0]

        answer = self.hcop.searchObjects(idobj, num, search, 0)  # 0 is locate

        print "found %d Cluster" % len(answer)
        return answer

    def find_and_grasp_tool(self):

        self.nav.drive_map_pose(look_for_tool_pose, self.hlo)
        # print "Reached map pose"

        self.hptu.look_at(self.hlo.getLOid("/kitchen-island-left")[0], True)
        self.sleep(2.0)  # Getting old data from Kinect
        look_more = True

        while look_more:

            # Look at the pancake
            fr = self.find_toolhandle()

            if fr is not None:
                grab_loid = self.hlo.update_frame("/base_link", "/spatula_handle", fr, 0)
                look_more = False

                self.rarm_action.move_to_reach("IceTea", "spatula", grab_loid)
                self.rarm_action.reach_primitive("IceTea", "spatula", grab_loid)

                # if the hand is empty, open_arm, look again
                # check grasp:
                def check_grasp(dist):
                    print "Finger grasp distance = %5.2f" % (dist)
                    # ~ 15 (grasp ok
                    # < 5 empty handed
                    # ~ 20 finger over thumb
                    # ~ 46 grasped 90deg off
                    if (dist > 5.0) and (dist < 20.0):
                        # grasped ok!
                        return True
                    else:
                        return False

                self.sleep(2.0)
                dist = self.rgrasper.getDistance(3)

                grasping_counter = 0

                good_grasp = check_grasp(dist)
                # good_grasp = True
                if not good_grasp:
                    # retry!
                    while (not good_grasp) and (grasping_counter < 2):
                        grasping_counter += 1
                        self.rarm_action.hand_primitive("open_relax")
                        self.sleep(3.0)
                        self.rarm_action.hand_primitive("spatula")
                        self.sleep(4.0)
                        dist = self.rgrasper.getDistance(5)
                        good_grasp = check_grasp(dist)

                        if not good_grasp:
                            self.sleep(1)

            else:
                print "Can't find the tool"
                # self.sleep(1)

    def move_to_manipulation_area(self):
        self.rarm_action.joint_pose("open")
        self.hb.cartesian_controller()
        f = kdl.Frame(kdl.Rotation.Quaternion(0.2027, 0.7216, 0.6267, 0.2130), kdl.Vector(0.950, 0.035, 1.350))
        self.arm.gotoFrameBlocking(f, 15)

    def random_vector(self, magnitude):
        return kdl.Vector(*[random.uniform(-magnitude, magnitude) for i in range(3)])

    def random_transform(self, rot_max, trans_max):
        while True:
            axis = self.random_vector(1.0)
            if axis.Norm() <= 1.0:
                break
        rot = kdl.Rotation.Rot(axis, random.uniform(0, rot_max))
        trans = self.random_vector(trans_max)
        return kdl.Frame(rot, trans)

    def random_movements(self, max_iter=20, rot_max=30 * pi / 180, trans_max=0.08):

        start_pose = self.arm.getCurrentFrame(5)

        self.hb.cartesian_controller()

        def checkPose():
            pose = self.arm.getCurrentFrame()
            return (pose.p - start_pose.p).Norm() > 0.05

        for i in range(max_iter):
            pose = start_pose * self.random_transform(rot_max, trans_max)
            print pose
            if not self.robsim.can_reach(pose):
                continue
            self.arm.gotoFrameBlocking(pose, self.single_movement_timeout, checkPose)
        self.arm.gotoFrameBlocking(start_pose, self.single_movement_timeout * 3, checkPose)

    def menu(self):
        go_on = True
        while go_on:
            try:
                print "Choose:"
                print "1. find and grasp tool"
                print "2. move to manipulation area"
                print "3. do random movements"
                print "rao. right-arm-open"
                print "lao. left-arm-open"
                print "lho. left-hand-open"
                print "rho. right-hand-open"
                print "lhc. left-hand-close"
                print "rhc. right-hand-close"
                print "0. exit"

                cmd = raw_input()

                if cmd == "1":
                    self.find_and_grasp_tool()
                elif cmd == "2":
                    self.move_to_manipulation_area()
                elif cmd == "3":
                    self.random_movements()
                elif cmd == "rao":
                    self.rarm_action.joint_pose("open")
                elif cmd == "lao":
                    self.larm_action.joint_pose("open")
                elif cmd == "0":
                    go_on = False
                elif cmd == "lho":
                    self.larm_action.hand_primitive("open_relax")
                elif cmd == "rho":
                    self.rarm_action.hand_primitive("open_relax")
                    weisswuerste_util.hide_wwcatcher_marker()
                elif cmd == "lhc":
                    self.larm_action.hand_primitive("spatula")
                elif cmd == "rhc":
                    self.rarm_action.hand_primitive("spatula")
                elif cmd == "drop":
                    self.drop_ww_in_a_bowl()
            except Exception, e:
                return