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