def ros_goal(self, userdata, default_goal): #import time #while True: # time.sleep(.01) return pm.Pr2GripperCommandGoal( pm.Pr2GripperCommand(position=self.gripper_size / 100., max_effort=self.effort))
def open(self, block, position=0.1, effort=-1): self.client.send_goal( pm.Pr2GripperCommandGoal( pm.Pr2GripperCommand(position=position, max_effort=effort))) if block: self.client.wait_for_result() return self.client.get_state()
def move(self, position, max_effort=-1): goal = pr2c.Pr2GripperCommandGoal( pr2c.Pr2GripperCommand(position=position, max_effort=max_effort)) self._ac.send_goal(goal) rospy.loginfo("Sending goal to gripper and waiting for result...") self._ac.wait_for_result() rospy.loginfo("Gripper action returned") if self._ac.get_state() != am.GoalStatus.SUCCEEDED: raise ex.ActionFailedError()
def set_gripper(self, grasp, max_effort=None, block=True): """ :param grasp: in meters :param max_effort: max force in Newtons :param block: if True, waits until grasp completed """ assert self.min_grasp <= grasp <= self.max_grasp max_effort = max_effort if max_effort is not None else self.default_max_effort self.gripper_command_client.send_goal(pcm.Pr2GripperCommandGoal(pcm.Pr2GripperCommand(position=grasp,max_effort=max_effort))) #self.gripper_command_pub.publish(pcm.Pr2GripperCommand(position=grasp,max_effort=max_effort)) if block: timeout = utils.Timeout(10) timeout.start() last_grasp = self.current_grasp + 1e-4 # slight offset to trigger loop while(np.abs(self.current_grasp - max(grasp,0)) > .005 and np.abs(last_grasp - self.current_grasp) > 1e-5 and not timeout.has_timed_out()): last_grasp = self.current_grasp rospy.sleep(0.5)
def set_angle(self, a, max_effort=default_max_effort): self.grip_client.send_goal( pcm.Pr2GripperCommandGoal( pcm.Pr2GripperCommand(position=a, max_effort=max_effort))) self.pr2.start_thread(Thread(target=self.grip_client.wait_for_result))