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