Python Gripper.gripping方法代碼示例

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import gripping [as 別名]
class Abbe_Gripper(object):

	def __init__(self):
		self._gripper = Gripper('left')
		self._gripper.calibrate()
        #don't want it to drop rfid reader so disabling right gripperq
		self._gripper_right = Gripper('right')
		self._gripper_right.calibrate()

	def close(self,left=True):
		if left:
			self._gripper.close(block=True)
		else:
			self._gripper_right.close(block=True)

	def open(self,left=True):
		if left:
			self._gripper.open(block=True)
		else:
			self._gripper_right.open(block=True)

	def gripping(self,left=True):
		if left:
			return self._gripper.gripping()
		else:
			return self._gripper_right.gripping()

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import gripping [as 別名]
class Abbe_Gripper(object):

	def __init__(self):
		self._gripper = Gripper('left')
		self._gripper.calibrate()
		#right gripper isn't calibrated so it won't drop the RFID reader
		#self._gripper_right = Gripper('right')
		#self._gripper_right.calibrate()

	def close(self,left=True):
		if left:
			self._gripper.close(block=True)
		else:
			self._gripper_right.close(block=True)

	def open(self,left=True):
		if left:
			self._gripper.open(block=True)
		else:
			self._gripper_right.open(block=True)

	def gripping(self,left=True):
		if left:
			return self._gripper.gripping()
		else:
			return self._gripper_right.gripping()

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import gripping [as 別名]

#.........這裏部分代碼省略.........
            dt.trajectory.append(rt)
        elif isinstance(trajectory, RobotState):
            dt.trajectory.append(js_to_rt(trajectory.joint_state))
        elif isinstance(trajectory, JointState):
            dt.trajectory.append(js_to_rt(trajectory))
        else:
            raise NotImplementedError("ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}".format(str(type(trajectory))))
        self._display_traj.publish(dt)

    def execute(self, trajectory, test=None, epsilon=0.1):
        """
        Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz)
        This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected
        Non-blocking needs should deal with the JointTrajectory action server
        :param trajectory: either a RobotTrajectory or a JointTrajectory
        :param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
        :param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality
        :return: True if execution ended successfully, False otherwise
        """
        def distance_to_first_point(point):
            joint_pos = np.array(point.positions)
            return np.linalg.norm(current_array - joint_pos)

        self.display(trajectory)
        with self._stop_lock:
            self._stop_reason = ''
        if isinstance(trajectory, RobotTrajectory):
            trajectory = trajectory.joint_trajectory
        elif not isinstance(trajectory, JointTrajectory):
            raise TypeError("Execute only accept RobotTrajectory or JointTrajectory")
        ftg = FollowJointTrajectoryGoal()
        ftg.trajectory = trajectory

        # check if it is necessary to move to the first point
        current = self.get_current_state()
        current_array = np.array([current.joint_state.position[current.joint_state.name.index(joint)] for joint in trajectory.joint_names])

        if distance_to_first_point(trajectory.points[0]) > epsilon:
            # convert first point to robot state
            rs = RobotState()
            rs.joint_state.name = trajectory.joint_names
            rs.joint_state.position = trajectory.points[0].positions
            # move to the first point
            self.move_to_controlled(rs)

        # execute the input trajectory
        self.client.send_goal(ftg)
        # Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory

        while self.client.simple_state != SimpleGoalState.DONE:
            if callable(test) and test():
                self.client.cancel_goal()
                return True

            if self._stop_reason!='':
                self.client.cancel_goal()
                return False

            self._rate.sleep()

        return True

    def close(self):
        """
        Open the gripper
        :return: True if an object has been grasped
        """
        return self._gripper.close(True)

    def open(self):
        """
        Close the gripper
        return: True if an object has been released
        """
        return self._gripper.open(True)

    def gripping(self):
        return self._gripper.gripping()

    def wait_for_human_grasp(self, threshold=1, rate=10, ignore_gripping=True):
        """
        Blocks until external motion is given to the arm
        :param threshold:
        :param rate: rate of control loop in Hertz
        :param ignore_gripping: True if we must wait even if no object is gripped
        """
        def detect_variation():
            new_effort = np.array(self.get_current_state([self.name+'_w0',
                                                          self.name+'_w1',
                                                          self.name+'_w2']).joint_state.effort)
            delta = np.absolute(effort - new_effort)
            return np.amax(delta) > threshold
        # record the effort at calling time
        effort = np.array(self.get_current_state([self.name+'_w0',
                                                  self.name+'_w1',
                                                  self.name+'_w2']).joint_state.effort)
        # loop till the detection of changing effort
        rate = rospy.Rate(rate)
        while not detect_variation() and not rospy.is_shutdown() and (ignore_gripping or self.gripping()):
            rate.sleep()