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)
