def _tighten_hand(self, pos_increment=0.8): self._disable_tactile_stops() pos_cmd = PoseCommand() pos_cmd.f1 = self.hand_state.finger[0].proximal + pos_increment pos_cmd.f2 = self.hand_state.finger[1].proximal + pos_increment pos_cmd.f3 = self.hand_state.finger[2].proximal + pos_increment pos_cmd.preshape = 0.0 for i in range(10): self.pos_cmd_pub.publish(pos_cmd)
def open_hand(self, req): pos_cmd = PoseCommand() pos_cmd.f1 = 0.0 pos_cmd.f2 = 0.0 pos_cmd.f3 = 0.0 pos_cmd.preshape = 0.0 for i in range(10): self.pos_cmd_pub.publish(pos_cmd) self.grasp_status = False return TriggerResponse(success=True)
import sys, rospy, cv2 from reflex_msgs.msg import PoseCommand from cibr_img_processing.msg import Ints global cmd, is_grasping is_grasping = False cmd = PoseCommand() cmd.f1 = 0.0 cmd.f2 = 0.0 cmd.f3 = 0.0 def vission_cb(data): global cmd cmd.preshape = (0.0 if data[0] else 2.0) if not is_grasping: command_pub.publish(cmd) def handy(): global cmd if not is_grasping: cmd.f1 = 0.8 cmd.f2 = 0.8 cmd.f3 = 0.8 command_pub.publish(cmd) is_grasping = True print("Grasping") else: cmd.f1 = 0.0 cmd.f2 = 0.0 cmd.f3 = 0.0