def move_home_init(self): # move_to_position([x_r,y_r,z_r], [0.072, 0.6902, -0.7172, 0.064]) move_to_position([self.x_r, self.y_r, self.z_r], [0.708, -0.019, 0.037, 0.705]) time.sleep(0.5) self.MOVING = True return 1
def move_home_init(self): # move_to_position([x_r,y_r,z_r], [0.072, 0.6902, -0.7172, 0.064]) move_to_position(KINOVA_HOME_XYZ, KINOVA_HOME_ORIENTATION) time.sleep(0.5) self.MOVING = True return 1
def robot_position_callback(msg): #只要 z 坐标 global SERVO global CURR_Z global CURR_DEPTH #global CURR_FORCE global VELO_COV global pose_averager global start_record_srv global stop_record_srv CURR_X = msg.pose.position.x # Stop Conditions. if CURR_Z < MIN_Z or (CURR_Z - 0.01) < GOAL_Z: #or CURR_FORCE < -5.0: if SERVO: SERVO = False # Grip. rospy.sleep(0.1) set_finger_positions([8000, 8000]) rospy.sleep(0.5) # Move Home. move_to_position([0, -0.38, 0.35], [0.99, 0, 0, np.sqrt(1 - 0.99**2)]) rospy.sleep(0.25) # stop_record_srv(std_srvs.srv.TriggerRequest()) raw_input('Press Enter to Complete') # Generate a control nonlinearity for this run. VELO_COV = generate_cartesian_covariance(0.0) # Open Fingers set_finger_positions([0, 0]) rospy.sleep(1.0) pose_averager.reset() raw_input('Press Enter to Start') # start_record_srv(std_srvs.srv.TriggerRequest()) rospy.sleep(0.5) SERVO = True
# https://github.com/dougsm/rosbag_recording_services # start_record_srv = rospy.ServiceProxy('/data_recording/start_recording', std_srvs.srv.Trigger) # stop_record_srv = rospy.ServiceProxy('/data_recording/stop_recording', std_srvs.srv.Trigger) # start_force_srv = rospy.ServiceProxy('/j2n6s300_driver/in/home_arm', kinova_msgs.srv.HomeArm)#go to home position # start_force_srv.call(kinova_msgs.srv.HomeArmRequest()) # Publish velocity at 100Hz. velo_pub = rospy.Publisher('/j2n6s300_driver/in/cartesian_velocity', kinova_msgs.msg.PoseVelocity, queue_size=1) #finger_pub = rospy.Publisher('/j2n6s300_driver/out/finger_position', kinova_msgs.msg.FingerPosition, queue_size=1) r = rospy.Rate(100) #start_force_srv = rospy.ServiceProxy('/j2n6s300_driver/in/start_force_control', kinova_msgs.srv.Start)#for safe's sake #start_force_srv.call(kinova_msgs.srv.StartRequest()) move_to_position([0, -0.3, 0.3], [0.99, 0, 0, np.sqrt(1 - 0.99**2)]) rospy.sleep(0.5) set_finger_positions([0, 0, 0]) rospy.sleep(0.5) SERVO = True while not rospy.is_shutdown(): if SERVO: #result = gripper_client(CURRENT_FINGER_VELOCITY) #finger_pub.publish(kinova_msgs.msg.FingerPosition(*CURRENT_FINGER_VELOCITY)) rospy.Duration(secs=0.01) velo_pub.publish(kinova_msgs.msg.PoseVelocity(*CURRENT_VELOCITY)) r.sleep()
def move_to_pose(pose): # Wrapper for move to position. p = pose.position o = pose.orientation move_to_position([p.x, p.y, p.z], [o.x, o.y, o.z, o.w])
queue_size=1) # https://github.com/dougsm/rosbag_recording_services # start_record_srv = rospy.ServiceProxy('/data_recording/start_recording', std_srvs.srv.Trigger) # stop_record_srv = rospy.ServiceProxy('/data_recording/stop_recording', std_srvs.srv.Trigger) # Enable/disable force control. start_force_srv = rospy.ServiceProxy( '/j2s7s300_driver/in/start_force_control', kinova_msgs.srv.Start) stop_force_srv = rospy.ServiceProxy( '/j2s7s300_driver/in/stop_force_control', kinova_msgs.srv.Stop) # Home position. raw_input('Press Enter to Move to Home Position.') # rospy.sleep(0.5) move_to_position([-0.05, -0.48, 0.35], [0.99, 0, 0, np.sqrt(1 - 0.99**2)]) print("******************") while not rospy.is_shutdown(): rospy.sleep(0.5) print("!!!!!!!!!!!!!!!!!!!") set_finger_positions([0, 0]) rospy.sleep(0.5) # raw_input('Press Enter to Start.') # start_record_srv(std_srvs.srv.TriggerRequest()) rospy.sleep(0.5) execute_grasp() move_to_position([0, -0.38, 0.25], [0.99, 0, 0, np.sqrt(1 - 0.99**2)])
def move_to_pose(pose): #包装器移动到位置。 p = pose.position o = pose.orientation move_to_position([p.x, p.y, p.z], [o.x, o.y, o.z, o.w])
def go_back(): global start_position global start_orientation move_to_position(start_position, start_orientation)