def setBackwardPose(self): rospy.logwarn("setInitPose") # init position joint_position = JointPosition() joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4'] #joint_position.position = [0.0, -1.05, 0.35, 0.70] joint_position.position = [0.0, -1.791, 0.507, 1.438] resp = False try: path_time = 2 resp = self.set_joint_position("", joint_position, path_time) rospy.sleep(path_time) except rospy.ServiceException as e: print("Service call failed: %s" % e) if not resp: return False # open gripper joint_position = JointPosition() joint_position.joint_name = ['gripper'] joint_position.position = [0.01] #-0.01 0.01 resp = False try: path_time = 1 resp = self.set_gripper_control("", joint_position, path_time) rospy.sleep(path_time) except rospy.ServiceException as e: print("Service call failed: %s" % e) if not resp: return False return True
def main(): rclpy.init(args=sys.argv) node = rclpy.create_node("iknet_inference") set_joint_position = node.create_client(SetJointPosition, "/goal_joint_space_path") parser = argparse.ArgumentParser() parser.add_argument( "--model", type=str, default="./iknet.pth", ) parser.add_argument("--trt", action="store_true", default=False) parser.add_argument("--x", type=float, default=0.1) parser.add_argument("--y", type=float, default=0.0) parser.add_argument("--z", type=float, default=0.1) parser.add_argument("--qx", type=float, default=0.0) parser.add_argument("--qy", type=float, default=0.0) parser.add_argument("--qz", type=float, default=0.0) parser.add_argument("--qw", type=float, default=1.0) args = parser.parse_args() device = torch.device("cuda" if torch.cuda.is_available() else "cpu") if not args.trt: model = IKNet() else: from torch2trt import TRTModule model = TRTModule() model.to(device) model.load_state_dict(torch.load(args.model)) model.eval() pose = [args.x, args.y, args.z, args.qx, args.qy, args.qz, args.qw] if not args.trt: input_ = torch.FloatTensor(pose) else: input_ = torch.FloatTensor([pose]) input_ = input_.to(device) print(f"input: {input_}") output = model(input_) print(f"output: {output}") joint_position = JointPosition() joint_position.joint_name = [f"joint{i+1}" for i in range(4)] if not args.trt: joint_position.position = [output[i].item() for i in range(4)] else: joint_position.position = [output[0][i].item() for i in range(4)] request = SetJointPosition.Request() request.joint_position = joint_position request.path_time = 4.0 future = set_joint_position.call_async(request) rclpy.spin_until_future_complete(node, future) if future.result() is not None: print(f"result: {future.result().is_planned}") else: print(f"exception: {future.exception()}") node.destroy_node() rclpy.shutdown()
def execute(self, userdata): inputfile = userdata.input_motionfile_file self.count = 0 while 1: if rospy.is_shutdown(): break if self.count > (len(inputfile) - 1): break if self.count == 0: position_path_time = 2 gripper_path_time = 1 operating_time = 2 else: position_path_time = 0.5 gripper_path_time = 0.5 operating_time = 0.2 try: joint_position = JointPosition() planning_group = userdata.input_planning_group joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] joint_position.position = [inputfile[self.count][0], inputfile[self.count][1], \ inputfile[self.count][2], inputfile[self.count][3]] path_time = position_path_time resp1 = self.set_joint_position(planning_group, joint_position, position_path_time) #print 'resp1 {}'.format(resp1.is_planned) #rospy.sleep(position_path_time) except rospy.ServiceException, e: print "Service call failed: %s" % e try: gripper_position = JointPosition() planning_group = userdata.input_planning_group gripper_position.joint_name = ['gripper'] if self.use_platform: gripper_position.position = [inputfile[self.count][4]] else: if inputfile[self.count][4] < 0.005: gripper_position.position = [-0.01] else: gripper_position.position = [0.01] path_time = gripper_path_time resp1 = self.set_gripper_position(planning_group, gripper_position, gripper_path_time) #print 'resp1 {}'.format(resp1.is_planned) #rospy.sleep(0.2) except rospy.ServiceException, e: print "Service call failed: %s" % e
def talker(): rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher' set_kinematics_position = rospy.ServiceProxy( '/goal_joint_space_path_to_kinematics_pose', SetKinematicsPose) set_gripper_position = rospy.ServiceProxy('/goal_tool_control', SetJointPosition) #while not rospy.is_shutdown(): kinematics_pose = KinematicsPose() kinematics_pose.pose.position.x = 0.2867 kinematics_pose.pose.position.y = 0.0 kinematics_pose.pose.position.z = 0.194 kinematics_pose.pose.orientation.w = 0.999 kinematics_pose.pose.orientation.x = 1.765 kinematics_pose.pose.orientation.y = 0.023 kinematics_pose.pose.orientation.z = 0.0 resp1 = set_kinematics_position('planning_group', 'gripper', kinematics_pose, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) rospy.sleep(3) kinematics_pose = KinematicsPose() kinematics_pose.pose.position.x = 0.0128 kinematics_pose.pose.position.y = 0.2748 kinematics_pose.pose.position.z = 0.1938 kinematics_pose.pose.orientation.w = 0.707 kinematics_pose.pose.orientation.x = -0.016 kinematics_pose.pose.orientation.y = 0.016 kinematics_pose.pose.orientation.z = 0.7058 resp1 = set_kinematics_position('planning_group', 'gripper', kinematics_pose, 3) rospy.sleep(3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3)
def joint_position_request_cb(userdata, request): joint = JointPosition() joint.position = userdata.input_position joint.max_velocity_scaling_factor = 1.0 joint.max_accelerations_scaling_factor = 1.0 request.planning_group = userdata.input_planning_group request.joint_position = joint return request
def arm_home(): #arm point left; gripper half close rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher' set_joint_position = rospy.ServiceProxy( '/open_manipulator/goal_joint_space_path', SetJointPosition) set_gripper_position = rospy.ServiceProxy( '/open_manipulator/goal_tool_control', SetJointPosition) joint_position = JointPosition() joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4'] joint_position.position = [0, -1.052, 0.377, 0.709] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.00 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) rospy.sleep(3)
def stop_movement(self, userdata): joint_position = JointPosition() joint_position.joint_name = ['joint1','joint2','joint3','joint4'] joint_position.position = [0, 0, 0, 0] planning_group = userdata.input_planning_group try: path_time = 1 resp1 = self.set_joint_position_from_present(planning_group,joint_position, path_time) rospy.sleep(path_time) except rospy.ServiceException, e: print "Service call failed: %s"%e
def talker(): rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher' set_joint_position = rospy.ServiceProxy('/goal_joint_space_path', SetJointPosition) set_gripper_position = rospy.ServiceProxy('/goal_tool_control', SetJointPosition) while not rospy.is_shutdown(): joint_position = JointPosition() joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4'] joint_position.position = [-0.5, 0, 0.5, -0.5] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback)
def test_rotate(): _qpose = JointPosition() _qpose.joint_name = ["joint1", "joint2", "joint3", "joint4"] _qpose.position = [0.5, 0.0, 0.0, 0.5] _qpose.max_accelerations_scaling_factor = 0.0 _qpose.max_velocity_scaling_factor = 0.0 try: task_space_srv = rospy.ServiceProxy( "/open_manipulator/goal_joint_space_path_from_present", SetJointPosition ) _ = task_space_srv("arm", _qpose, 2.0) except rospy.ServiceException, e: rospy.loginfo("Path planning service call failed: {0}".format(e))
def initAction(self,userdata): try: joint_position = JointPosition() planning_group = userdata.input_planning_group joint_position.joint_name = ['joint1','joint2','joint3','joint4'] joint_position.position = [0.0, -1.05, 0.35, 0.70] path_time = 5 resp1 = self.set_joint_position(planning_group,joint_position, path_time) #print 'resp1 {}'.format(resp1.is_planned) rospy.sleep(5) except rospy.ServiceException, e: print "Service call failed: %s"%e
def moveToPlace(self): rospy.logwarn("move_to_place") # close gripper joint_position = JointPosition() joint_position.joint_name = ['gripper'] joint_position.position = [-0.01] #-0.01 0.01 resp = False try: path_time = 1 resp = self.set_gripper_control("",joint_position, path_time) rospy.sleep(path_time) except rospy.ServiceException, e: print "Service call failed: %s"%e
def panoramaShot(self,userdata): print 'panoramaShot init point\n' try: joint_position = JointPosition() planning_group = userdata.input_planning_group joint_position.joint_name = ['joint1','joint2','joint3','joint4'] joint_position.position = [-1.57, -1.049, 0.354, 0.715] path_time = 5 resp1 = self.set_joint_position(planning_group,joint_position, path_time) #print 'resp1 {}'.format(resp1.is_planned) rospy.sleep(5) except rospy.ServiceException, e: print "Service call failed: %s"%e
def setBackwardPose2(self): rospy.logwarn("setInitPose") # init position joint_position = JointPosition() joint_position.joint_name = ['joint1','joint2','joint3','joint4'] #joint_position.position = [0.0, -1.05, 0.35, 0.70] joint_position.position = [0.0, -1.791, 0.507, 1.438] resp = False try: path_time = 2 resp = self.set_joint_position("",joint_position, path_time) rospy.sleep(path_time) except rospy.ServiceException, e: print "Service call failed: %s"%e
def facetrackingActionByJoint(self,userdata): #center_x = 112 #320 center_x = 182 #320 center_y = 158 #240 x_diff = (center_x - self.face_center_x) y_diff = (center_y - self.face_center_y) #move_x = (x_diff*1000/320)*0.1/1000 #move_y = (y_diff*1000/320)*-0.05/1000 move_x = (x_diff*1000/320)*0.3/1000 move_y = (y_diff*1000/320)*-0.15/1000 #rospy.logwarn('C_XY %.3f,%.3f , XY_Diff %.3f,%.3f move_xy %.3f,%.3f)', self.face_center_x ,self.face_center_y , \ #x_diff, y_diff, move_x, move_y) object_detect_duration = rospy.get_rostime().to_sec() - self.last_detect_time.to_sec() #rospy.logwarn("duration %.2f",object_detect_duration ) #check last face detected time if object_detect_duration > 1 : rospy.logwarn(" no face.. %d ", self.no_detect_count) self.last_no_detect_time = rospy.get_rostime() self.no_detect_count = self.no_detect_count + 1 if self.no_detect_count > 30 : rospy.logwarn(" no_detect_count too long..........................set init!!!!!!!!!!!!!!!!!") print 'set init position for face tracking \n' try: joint_position = JointPosition() planning_group = userdata.input_planning_group joint_position.joint_name = ['joint1','joint2','joint3','joint4'] joint_position.position = [0.0, -1.05, 0.35, 0.70] path_time = 5 resp1 = self.set_joint_position(planning_group,joint_position, path_time) #print 'resp1 {}'.format(resp1.is_planned) rospy.sleep(5) except rospy.ServiceException, e: print "Service call failed: %s"%e self.no_detect_count = 0 rospy.sleep(0.1) return ;
def arm_home(): #arm point left; gripper close rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher' set_kinematics_position = rospy.ServiceProxy( '/open_manipulator/goal_joint_space_path_to_kinematics_position', SetKinematicsPose) set_gripper_position = rospy.ServiceProxy( '/open_manipulator/goal_tool_control', SetJointPosition) kinematics_pose = KinematicsPose() kinematics_pose.pose.position.x = 0.15 kinematics_pose.pose.position.y = 0.01 kinematics_pose.pose.position.z = 0.2 resp1 = set_kinematics_position('planning_group', 'gripper', kinematics_pose, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.0 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) rospy.sleep(3)
def talker(): rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher' set_kinematics_position = rospy.ServiceProxy( '/goal_joint_space_path_to_kinematics_position', SetKinematicsPose) set_gripper_position = rospy.ServiceProxy('/goal_tool_control', SetJointPosition) #while not rospy.is_shutdown(): kinematics_pose = KinematicsPose() kinematics_pose.pose.position.x = 0.287 kinematics_pose.pose.position.y = 0.0 kinematics_pose.pose.position.z = 0.194 resp1 = set_kinematics_position('planning_group', 'gripper', kinematics_pose, 3) sub_kinematics_pose = rospy.Subscriber('/gripper/kinematics_pose', KinematicsPose, callback) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) rospy.sleep(3) kinematics_pose = KinematicsPose() kinematics_pose.pose.position.x = 0.0 kinematics_pose.pose.position.y = 0.276 kinematics_pose.pose.position.z = 0.192 resp1 = set_kinematics_position('planning_group', 'gripper', kinematics_pose, 3) rospy.sleep(3) sub_kinematics_pose = rospy.Subscriber('/gripper/kinematics_pose', KinematicsPose, callback)
def execute(self, userdata): print "Send Position by Joint Control " joint_position = JointPosition() joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4'] joint_position.position = [0, 0, 0, 0] planning_group = userdata.input_planning_group end_effector_name = "gripper" kinematics_pose = KinematicsPose() cup_tf = [0, 0, 0] self.mode = 9 self.step = self.StepCupTracking.waiting_signal.value while 1: rospy.logwarn("waiting_signal %d ", ) if rospy.is_shutdown(): return 'aborted' elif self.mode == 2: return 'succeeded' elif self.mode == 6: self.stop_movement(userdata) self.step = self.StepCupTracking.waiting_signal.value pass if self.step == self.StepCupTracking.waiting_signal.value: #rospy.logwarn("waiting_signal") if self.mode == 7: self.pre_step = self.StepCupTracking.waiting_signal.value self.step = self.StepCupTracking.go_backward_location.value else: self.step = self.StepCupTracking.go_backward_location.value pass if self.step == self.StepCupTracking.go_backward_location.value: rospy.logwarn("go_backward_location") #rospy.sleep(3) joint_position.position = [0, -1.551, -0.234, 1.98] path_time = 2.5 try: resp1 = self.set_joint_position(planning_group, joint_position, path_time) #print 'resp1 {}'.format(resp1.is_planned) rospy.sleep(path_time) except rospy.ServiceException, e: print "Service call failed: %s" % e self.pre_step = self.StepCupTracking.go_backward_location.value self.step = self.StepCupTracking.wait_detecting_tf_of_cup.value elif self.step == self.StepCupTracking.wait_detecting_tf_of_cup.value: #rospy.logwarn("wait_detecting_tf_of_cup") rospy.sleep(3) object_detect_duration = rospy.get_rostime().to_sec( ) - self.last_detect_time.to_sec() #rospy.logwarn("duration %.2f",object_detect_duration ) tmp_x = self.trans[0] tmp_y = self.trans[1] tmp_z = self.trans[2] distance = math.sqrt(tmp_x**2 + tmp_y**2 + tmp_z**2) width = self.cupboundingBox.xmax - self.cupboundingBox.xmin if tmp_z < 0.06: tmp_z = 0.05 #rospy.sleep(0.5) if self.use_platform: if object_detect_duration > 1 or tmp_x > 0.5 or distance > 0.7 or tmp_x < 0.07 or self.cupboundingBox.Z < 0.01: rospy.logwarn("object_detect_duration1 %.2f,tmp_x %.2f,distance %.2f,tmp_z %.2f,tmp_x %.2f, self.cupboundingBox.Z %.2f", \ object_detect_duration,tmp_x,distance,tmp_z,tmp_x, self.cupboundingBox.Z) rospy.logwarn(" no object1.. ") pass else: radian = math.atan(tmp_y / tmp_x) degree = math.degrees(radian) #dist = 0.1 dist = 0.07 distX = math.cos(radian) * dist distY = math.sin(radian) * dist rospy.logwarn( "tmp_xyz1 %.2f,%.2f,%.2f _ radian %.2f(%.2f) distXY %.3f , %.3f", tmp_x, tmp_y, tmp_z, radian, degree, distX, distY) cup_tf = [tmp_x - distX, tmp_y - distY, tmp_z + 0.05] self.pre_step = self.StepCupTracking.wait_detecting_tf_of_cup.value self.step = self.StepCupTracking.go_base_location.value else: if object_detect_duration > 1 or tmp_x > 0.5 or distance > 0.7 or self.cupboundingBox.Z < 0.01: rospy.logwarn("object_detect_duration2 %.2f,tmp_x %.2f,distance %.2f,tmp_z %.2f,tmp_x %.2f, self.cupboundingBox.Z %.2f", \ object_detect_duration,tmp_x,distance,tmp_z,tmp_x, self.cupboundingBox.Z) rospy.logwarn(" no object2.. ") pass else: radian = math.atan(tmp_y / tmp_x) degree = math.degrees(radian) dist = 0.038 distX = math.cos(radian) * dist distY = math.sin(radian) * dist rospy.logwarn( "tmp_xyz2 %.2f,%.2f,%.2f _ radian %.2f(%.2f) distXY %.3f , %.3f", tmp_x, tmp_y, tmp_z, radian, degree, distX, distY) cup_tf = [tmp_x - distX, tmp_y - distY, tmp_z] self.pre_step = self.StepCupTracking.wait_detecting_tf_of_cup.value self.step = self.StepCupTracking.go_base_location.value
rospy.logwarn("go xyz %.2f,%.2f,%.2f , moveDistance %.2f, operate time %.2f ( %.2f )" ,\ kinematics_pose.pose.position.x, kinematics_pose.pose.position.y, kinematics_pose.pose.position.z, \ moveDistance, operating_time , operating_limit_time) try: resp = self.set_kinematics_position(planning_group, end_effector_name, kinematics_pose, operating_time) rospy.logwarn( 'kinemetics resp1 {} time '.format(resp.is_planned, operating_time)) rospy.sleep(operating_time) except rospy.ServiceException, e: rospy.logwarn( "Service call failed: %s"%e) return False # open gripper joint_position = JointPosition() joint_position.joint_name = ['gripper'] joint_position.position = [0.01] #-0.01 0.01 resp = False try: path_time = 1 resp = self.set_gripper_control("",joint_position, path_time) rospy.sleep(path_time) except rospy.ServiceException, e: print "Service call failed: %s"%e if not resp : return False return True def forwardObjectPosition( self, objectPosition, forward_distance ): resultPoint = Point() if(abs(objectPosition.x) < 0.001) :
def talker(): rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher' set_joint_position = rospy.ServiceProxy('/goal_joint_space_path', SetJointPosition) set_gripper_position = rospy.ServiceProxy('/goal_tool_control', SetJointPosition) joint_position = JointPosition() joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4'] #Step 1 joint_position.position = [0, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) rospy.sleep(3) #Step 2 joint_position.position = [1.571, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #Step 3 joint_position.position = [1.571, 0.526, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #Step 4 gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) rospy.sleep(3) #Step 5 joint_position.position = [1.571, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #Step 6 joint_position.position = [0, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #Step 7 joint_position.position = [-1.571, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #Step 8 joint_position.position = [-1.571, 0.526, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #Step 7b gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) rospy.sleep(3) #bring arm up joint_position.position = [-1.571, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #initial pose joint_position.position = [0, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3)
def talker(): rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher' set_joint_position = rospy.ServiceProxy('/goal_joint_space_path', SetJointPosition) set_gripper_position = rospy.ServiceProxy('/goal_tool_control', SetJointPosition) while not rospy.is_shutdown(): ii = 0 while ii < 50: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians #joint_position.position = [3.1416, 0.8674, 0.1562, -1.2435] # in radians joint_position.position = [0.78, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) #rospy.sleep(0.1) while ii < 250: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians #joint_position.position = [3.1416, 0.8674, 0.1562, -1.2435] # in radians joint_position.position = [1.5706, -0.2, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.03) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians #joint_position.position = [1.570, 0.8674, 0.1562, -1.2435] # in radians joint_position.position = [1.570, 0.7639, 0.2141, -0.9771] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.02) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians #joint_position.position = [1.570, 0.8674, 0.1562, -1.2435] # in radians joint_position.position = [1.570, 0.7639, 0.2141, -0.9771] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.01) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [1.570, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.001) joint_position = JointPosition() joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4'] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [0, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.001) ii = 0 while ii < 100: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [-1.57, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.1) ii = 0 while ii < 100: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians #joint_position.position = [-1.57, -0.5, 0, 0] # in radians joint_position.position = [-1.570, 0.7639, 0.2141, -0.9771] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.02) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians #joint_position.position = [-1.57, 0.8674, 0.1562, -1.2435] # in radians joint_position.position = [-1.570, 0.7639, 0.2141, -0.9771] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.1) ii = 0 while ii < 100: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians #joint_position.position = [-1.57, -0.5, 0, 0] # in radians joint_position.position = [-1.570, 0.7639, 0.2141, -0.9771] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.001) ii = 0 while ii < 100: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [-1.57, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.001) joint_position = JointPosition() joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4'] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [0, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.01) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [1.570, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.001) ii = 0 while ii < 100: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [1.570, 0.7639, 0.2141, -0.9] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.01) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [1.570, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.01) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [0, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.01) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [-1.570, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.001) ii = 0 while ii < 100: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [-1.570, 0.7639, 0.2141, -0.9] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.01) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [-1.570, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.01) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [0, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.01) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [1.57, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.001) ii = 0 while ii < 100: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [1.570, 0.7639, 0.2141, -0.9] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [1.57, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [0, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) ii = 0 while ii < 200: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [-1.57, -0.5, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback) rospy.sleep(0.001) ii = 0 while 1: #ii<100: ii += 1 joint_position = JointPosition() joint_position.joint_name = [ 'joint1', 'joint2', 'joint3', 'joint4' ] #joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians joint_position.position = [-1.570, 0.7639, 0.2141, -0.9] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) sub_joint_state = rospy.Subscriber('/joint_states', JointState, callback)
def talker(): rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher' set_joint_position = rospy.ServiceProxy('/goal_joint_space_path', SetJointPosition) set_gripper_position = rospy.ServiceProxy('/goal_tool_control', SetJointPosition) # Loop until ^c; comment this line if you want the arm to stop at the last joint set #while not rospy.is_shutdown(): joint_position = JointPosition() joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4'] #Step 1 joint_position.position = [0, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) gripper_position = JointPosition() gripper_position.joint_name = ['gripper'] gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) rospy.sleep(3) #Step 2 joint_position.position = [1.571, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #Step 3a joint_position.position = [1.571, 0.526, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #Step 3b gripper_position.position = [ -0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) rospy.sleep(3) #Step 4 joint_position.position = [1.571, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #Step 5 joint_position.position = [0, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #Step 6 joint_position.position = [-1.571, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #Step 7a joint_position.position = [-1.571, 0.526, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #Step 7b gripper_position.position = [ 0.01 ] # -0.01 for fully close and 0.01 for fully open respg2 = set_gripper_position('planning_group', gripper_position, 3) rospy.sleep(3) #bring arm up joint_position.position = [-1.571, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3) rospy.sleep(3) #initial pose joint_position.position = [0, 0, 0, 0] # in radians resp1 = set_joint_position('planning_group', joint_position, 3)
#center range set if abs(x_diff) > center_range_x or abs(y_diff) > center_range_y : #operating range set _ x range (-90 degree ~ 90 degree) y range (-45 degree ~ 45 degree) if (self.jointStates[0] > 1.57 and move_x > 0) or (self.jointStates[0] < -1.57 and move_x < 0 ) : move_x = 0 #rospy.logwarn("set joint0 state %.3f , move_x %.3f",self.jointStates[0], move_x) if (self.jointStates[3] > 1.57 and move_y > 0) or (self.jointStates[3] < 0 and move_y < 0 ) : move_y = 0 #rospy.logwarn("set joint3 state %.3f , move_y %.3f",self.jointStates[3], move_y) #tracking action try: joint_position = JointPosition() planning_group = userdata.input_planning_group joint_position.joint_name = ['joint1','joint2','joint3','joint4'] joint_position.position = [move_x, 0, 0, move_y] resp1 = self.set_joint_position_from_present(planning_group,joint_position, 0.5) rospy.sleep(0.2) print 'resp2 {}'.format(resp1.is_planned) except rospy.ServiceException, e: print "Service call failed: %s"%e def panoramaShot(self,userdata): print 'panoramaShot init point\n' try: joint_position = JointPosition() planning_group = userdata.input_planning_group joint_position.joint_name = ['joint1','joint2','joint3','joint4'] joint_position.position = [-1.57, -1.049, 0.354, 0.715] path_time = 5