Ejemplo n.º 1
0
 def __init__(self, limb):
     self.limb = limb
     self.interface = baxter_interface.Limb(limb)
     self.solver = IKSolver(limb)
     self.last_solve_request_time = rospy.Time.now()
     self.running = True
     self.thread = threading.Thread(target=self._update_thread)
     self.vis = Vis()
     self.goal_transform = GoalTransform(limb)
class LimbMover(object):
    def __init__(self, limb):
        self.limb = limb
        self.interface = baxter_interface.Limb(limb)
        self.interface.set_joint_position_speed(0.5)
        self.solver = IKSolver(limb)
        self.last_solve_request_time = rospy.Time.now()
        self.running = True
        self.thread = threading.Thread(target=self._update_thread)
        self.vis = Vis(limb)
        self.goal_transform = GoalTransform(limb)
        self._lock = threading.Lock()

    def enable(self):
        self.thread.start()

    def set_target(self, joints):
        self.target_joints = joints

    def _update_req_time(self):
        self.last_solve_request_time = rospy.Time.now()

    def _solver_cooled_down(self):
        time_since_req = rospy.Time.now() - self.last_solve_request_time
        return time_since_req > rospy.Duration(0.05)  # 20 Hz

    def update(self, trigger, gripper_travel):
        
        self.vis.show_gripper(gripper_travel)
        self.goal_transform.update()
        
        # Throttle service requests
        if trigger and self._solver_cooled_down():
            self._update_req_time()
            with self._lock:            
                return self.solver.solve()
        return True

    def stop_thread(self):
        if self.thread.is_alive():
            self.running = False
            self.thread.join()

    def _update_thread(self):
        rospy.loginfo("Starting Joint Update Thread: %s\n" % self.limb)
        rate = rospy.Rate(200)
        while not rospy.is_shutdown() and self.running:
            with self._lock:
                self.interface.set_joint_positions(self.solver.solution)
            rate.sleep()
        rospy.loginfo("Stopped %s" % self.limb)
Ejemplo n.º 3
0
class LimbMover(object):
    def __init__(self, limb):
        self.limb = limb
        self.interface = baxter_interface.Limb(limb)
        self.solver = IKSolver(limb)
        self.last_solve_request_time = rospy.Time.now()
        self.running = True
        self.thread = threading.Thread(target=self._update_thread)
        self.vis = Vis()
        self.goal_transform = GoalTransform(limb)

    def enable(self):
        self.thread.start()

    def set_target(self, joints):
        self.target_joints = joints

    def _update_req_time(self):
        self.last_solve_request_time = rospy.Time.now()

    def _solver_cooled_down(self):
        time_since_req = rospy.Time.now() - self.last_solve_request_time
        return time_since_req > rospy.Duration(0.05)  # 20 Hz

    def update(self, trigger, gripper_travel):
        self.vis.show_gripper(self.limb, gripper_travel, 0.026, 0.11, 1)
        self.goal_transform.update()

        # Throttle service requests
        if trigger and self._solver_cooled_down():
            self._update_req_time()
            return self.solver.solve()
        return True

    def stop_thread(self):
        if self.thread.is_alive():
            self.running = False
            self.thread.join()

    def _update_thread(self):
        rospy.loginfo("Starting Joint Update Thread: %s\n" % self.limb)
        rate = rospy.Rate(200)
        while not rospy.is_shutdown() and self.running:
            self.interface.set_joint_positions(self.solver.solution)
            rate.sleep()
        rospy.loginfo("Stopped %s" % self.limb)
Ejemplo n.º 4
0
 def __init__(self, limb):
     self.limb = limb
     self.interface = baxter_interface.Limb(limb)
     self.solver = IKSolver(limb)
     self.last_solve_request_time = rospy.Time.now()
     self.running = True
     self.thread = threading.Thread(target=self._update_thread)
     self.vis = Vis()
     self.goal_transform = GoalTransform(limb)