def __init__(self, arm, joint_names, joint_topic):
RobotController.__init__(self, joint_names, joint_topic)
self.arm = arm
# Set smoothness of angle filter
self.coef = rospy.get_param('~filter_coef', 0.1)
# Adds API support for controlling Baxter arm
self.limb = baxter_interface.Limb(self.arm)
# Adds API support for controlling Baxter gripper
self.gripper = baxter_interface.Gripper(self.arm)
def __init__(self, control_rate, robot_name):
self.max_release = 0
RobotController.__init__(self)
self.sem_list = [Semaphore(value=0)]
self._status_mutex = Lock()
self.robot_name = robot_name
self._desired_gpos = GRIPPER_OPEN
self.gripper_speed = 300
self._force_counter = 0
self._integrate_gripper_force = 0.
self.num_timeouts = 0
self.gripper_pub = rospy.Publisher('/wsg_50_driver/goal_position',
Cmd,
queue_size=10)
rospy.Subscriber("/wsg_50_driver/status", Status,
self._gripper_callback)
print("waiting for first status")
self.sem_list[0].acquire()
print('gripper initialized!')
self.imp_ctrl_publisher = rospy.Publisher('/desired_joint_pos',
JointState,
queue_size=1)
self.imp_ctrl_release_spring_pub = rospy.Publisher('/release_spring',
Float32,
queue_size=10)
self.imp_ctrl_active = rospy.Publisher('/imp_ctrl_active',
Int64,
queue_size=10)
self.control_rate = rospy.Rate(control_rate)
self.imp_ctrl_release_spring(100)
self.imp_ctrl_active.publish(1)
self._navigator = intera_interface.Navigator()
self._navigator.register_callback(self._close_gripper_handler,
'right_button_ok')
