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)
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)