def pick_Towel(self): self.calibrate_obj_det_pub.publish(True) while self.calibrated == False: pass self.calibrate_obj_det_pub.publish(False) print("Finger Sensors calibrated") if self.listen.frameExists("/root") and self.listen.frameExists( "/Towel_position"): print("we have the spoon frame") self.listen.waitForTransform('/root', '/Towel_position', rospy.Time(), rospy.Duration(100.0)) t = self.listen.getLatestCommonTime("/root", "/Towel_position") translation, quaternion = self.listen.lookupTransform( "/root", "/Towel_position", t) translation = list(translation) quaternion = list(quaternion) pose_value = translation + quaternion # print (quaternion) orientation_XYZ = pose_action_client.Quaternion2EulerXYZ( quaternion) # self.j.gripper.open() #second arg=0 (absolute movement), arg = '-r' (relative movement) self.cmmnd_CartesianPosition(pose_value, 0)
def goto_straw(self): self.calibrate_obj_det_pub.publish(True) while self.calibrated == False: pass self.calibrate_obj_det_pub.publish(False) print("Finger Sensors calibrated") if self.listen.frameExists("/root") and self.listen.frameExists("/straw_position"): print ("we have the straw frame") self.listen.waitForTransform('/root','/straw_position',rospy.Time(),rospy.Duration(100.0)) t = self.listen.getLatestCommonTime("/root", "/straw_position") translation, quaternion = self.listen.lookupTransform("/root", "/straw_position", t) translation = list(translation) quaternion = list(quaternion) pose_value = translation + quaternion orientation_XYZ = pose_action_client.Quaternion2EulerXYZ(quaternion) self.cmmnd_CartesianPosition(pose_value, 0) else: print ("We DONT have the frame")
def cmmnd_CartesianPosition(self, pose_value, relative): pose_action_client.getcurrentCartesianCommand('j2n6a300_') pose_mq, pose_mdeg, pose_mrad = pose_action_client.unitParser('mq', pose_value, relative) poses = [float(n) for n in pose_mq] orientation_XYZ = pose_action_client.Quaternion2EulerXYZ(poses[3:]) try: poses = [float(n) for n in pose_mq] result = pose_action_client.cartesian_pose_client(poses[:3], poses[3:]) except rospy.ROSInterruptException: print ("program interrupted before completion")
def goto_syringe(self): print ("we are in fuctn") if self.listen.frameExists("/root") and self.listen.frameExists("/syringe_position"): print ("we have the syringe_position frame") self.listen.waitForTransform('/root','/syringe_position',rospy.Time(),rospy.Duration(100.0)) t = self.listen.getLatestCommonTime("/root", "/syringe_position") translation, quaternion = self.listen.lookupTransform("/root", "/syringe_position", t) translation = list(translation) quaternion = list(quaternion) pose_value = translation + quaternion # print (quaternion) orientation_XYZ = pose_action_client.Quaternion2EulerXYZ(quaternion) # self.j.gripper.open() #second arg=0 (absolute movement), arg = '-r' (relative movement) self.cmmnd_CartesianPosition(pose_value, 0)