def _gripper_callback(self, status): # print('callback! list-len {}, max_release {}'.format(len(self.sem_list), self.max_release)) self._status_mutex.acquire() self._gripper_width, self._gripper_force = status.width, status.force self._integrate_gripper_force += status.force self._force_counter += 1 cmd = Cmd() cmd.pos = self._desired_gpos cmd.speed = self.gripper_speed self.gripper_pub.publish(cmd) if len(self.sem_list) > 0: gripper_close = np.isclose(self._gripper_width, self._desired_gpos, atol=1e-1) if gripper_close or self._gripper_force > 0 or self.max_release > 15: if self.max_release > 15: self.num_timeouts += 1 for s in self.sem_list: s.release() self.sem_list = [] self.max_release += 1 #timeout for when gripper responsive but can't acheive commanded state else: self.max_release = 0 self._status_mutex.release()
def set_weiss_gripper(self, des_pos): """ :param des_pos: :return: """ cmd = Cmd() cmd.speed = 100. cmd.pos = des_pos # print('command weiss', cmd.pos) self.weiss_pub.publish(cmd)
def graspmove_nopending(self, width, speed=20): '''move to the objective position 'width', while stop if the force limit acquired return before the position reached''' cmd = Cmd() cmd.mode = 'Script' cmd.pos = width cmd.speed = speed pub = rospy.Publisher('/wsg_50_driver/goal_position', Cmd, queue_size=10) pub.publish(cmd)
def execute_cb(self, goal): gripper_command = Cmd() ### Driver simulator do not implement speed regulation gripper_command.speed = 1 if goal.goal == GraspHandPostureExecutionGoal.GRASP: if not goal.grasp.grasp_posture.position: rospy.logerr( "WSG grasp controller: position vector empty in requested grasp" ) self.action_server.set_aborted() return gripper_command.mode = GraspHandPostureExecutionGoal.GRASP elif goal.goal == GraspHandPostureExecutionGoal.PRE_GRASP: if not goal.grasp.pre_grasp_posture.position: rospy.logerr( "WSG grasp controller: position vector empty in requested pre_grasp" ) self.action_server.set_aborted() return gripper_command.mode = GraspHandPostureExecutionGoal.PRE_GRASP gripper_command.pos = goal.grasp.pre_grasp_posture.position[ 1] * 2 * 1000 # in mm elif goal.goal == GraspHandPostureExecutionGoal.RELEASE: gripper_command.mode = GraspHandPostureExecutionGoal.RELEASE gripper_command.pos = self.gripper_opened_gap_value else: rospy.logerr("WSG grasp controller: unknown goal code (%d)" % goal.goal) self.action_server.set_aborted() return if gripper_command.mode == GraspHandPostureExecutionGoal.GRASP: response = self.gripper_grasp_srv(gripper_command.pos, gripper_command.speed) else: response = self.gripper_move_srv(gripper_command.pos, gripper_command.speed) if response == 0: self.action_server.set_succeeded() else: rospy.logwarn( "WSG grasp controller: gripper goal not achieved for pre-grasp or release" ) self.action_server.set_succeeded()
def execute_cb(self, goal): gripper_command = Cmd() ### Driver simulator do not implement speed regulation gripper_command.speed = 1 if goal.goal == GraspHandPostureExecutionGoal.GRASP: if not goal.grasp.grasp_posture.position: rospy.logerr("WSG grasp controller: position vector empty in requested grasp") self.action_server.set_aborted() return gripper_command.mode = GraspHandPostureExecutionGoal.GRASP elif goal.goal == GraspHandPostureExecutionGoal.PRE_GRASP: if not goal.grasp.pre_grasp_posture.position: rospy.logerr("WSG grasp controller: position vector empty in requested pre_grasp") self.action_server.set_aborted() return gripper_command.mode = GraspHandPostureExecutionGoal.PRE_GRASP gripper_command.pos = goal.grasp.pre_grasp_posture.position[1] * 2 * 1000 # in mm elif goal.goal == GraspHandPostureExecutionGoal.RELEASE: gripper_command.mode = GraspHandPostureExecutionGoal.RELEASE gripper_command.pos = self.gripper_opened_gap_value else: rospy.logerr("WSG grasp controller: unknown goal code (%d)" % goal.goal) self.action_server.set_aborted() return if gripper_command.mode == GraspHandPostureExecutionGoal.GRASP: response = self.gripper_grasp_srv(gripper_command.pos, gripper_command.speed) else: response = self.gripper_move_srv(gripper_command.pos, gripper_command.speed) if response == 0: self.action_server.set_succeeded() else: rospy.logwarn("WSG grasp controller: gripper goal not achieved for pre-grasp or release") self.action_server.set_succeeded()
def set_weiss_griper(self, width): cmd = Cmd() cmd.pos = width cmd.speed = 100. self.weiss_pub.publish(cmd)
def ctrl_callback(self, status): print('status', status) cmd = Cmd() cmd.pos = 4. cmd.speed = 100. self.pub.publish(cmd)