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
def execute_grasp():
# Execute a grasp.
global MOVING
global CURR_Z
global start_force_srv
global stop_force_srv
# Get the positions.
msg = rospy.wait_for_message('/ggcnn/out/command',
std_msgs.msg.Float32MultiArray)
d = list(msg.data)
# Calculate the gripper width.
grip_width = d[4]
# Convert width in pixels to mm.
# 0.07 is distance from end effector (CURR_Z) to camera.
# 0.1 is approx degrees per pixel for the realsense.
#g_width = 2 * ((CURR_Z + 0.07)) * np.tan(0.1 * grip_width / 2.0 / 180.0 * np.pi) * 1000
# Convert into motor positions.
#g = min((1 - (min(g_width, 70)/70)) * (6800-4000) + 4000, 5500)
#set_finger_positions([g, g])
rospy.sleep(0.5)
# Pose of the grasp (position only) in the camera frame.
gp = geometry_msgs.msg.Pose()
gp.position.x = d[0]
gp.position.y = d[1]
gp.position.z = d[2]
gp.orientation.w = 1
# Convert to base frame, add the angle in (ensures planar grasp, camera isn't guaranteed to be perpendicular).
gp_base = convert_pose(gp, 'camera_depth_optical_frame',
'm1n6s200_link_base')
q = tft.quaternion_from_euler(np.pi, 0, d[3])
gp_base.orientation.x = q[0]
gp_base.orientation.y = q[1]
gp_base.orientation.z = q[2]
gp_base.orientation.w = q[3]
publish_pose_as_transform(gp_base, 'm1n6s200_link_base', 'G', 0.5)
# Offset for initial pose.
initial_offset = 0.20
gp_base.position.z += initial_offset
# Disable force control, makes the robot more accurate.
stop_force_srv.call(kinova_msgs.srv.StopRequest())
move_to_pose(gp_base)
rospy.sleep(0.1)
# Start force control, helps prevent bad collisions.
start_force_srv.call(kinova_msgs.srv.StartRequest())
rospy.sleep(0.25)
# Reset the position
gp_base.position.z -= initial_offset
# Flag to check for collisions.
MOVING = True
# Generate a nonlinearity for the controller.
cart_cov = generate_cartesian_covariance(0)
# Move straight down under velocity control.
velo_pub = rospy.Publisher('/m1n6s200_driver/in/cartesian_velocity',
kinova_msgs.msg.PoseVelocity,
queue_size=1)
while MOVING and CURR_Z - 0.02 > gp_base.position.z:
dz = gp_base.position.z - CURR_Z - 0.03 # Offset by a few cm for the fingertips.
MAX_VELO_Z = 0.08
dz = max(min(dz, MAX_VELO_Z), -1.0 * MAX_VELO_Z)
v = np.array([0, 0, dz])
vc = list(np.dot(v, cart_cov)) + [0, 0, 0]
velo_pub.publish(kinova_msgs.msg.PoseVelocity(*vc))
rospy.sleep(1 / 100.0)
MOVING = False
# close the fingers.
rospy.sleep(0.1)
set_finger_positions([8000, 8000])
rospy.sleep(0.5)
# Move back up to initial position.
gp_base.position.z += initial_offset
gp_base.orientation.x = 1
gp_base.orientation.y = 0
gp_base.orientation.z = 0
gp_base.orientation.w = 0
move_to_pose(gp_base)
stop_force_srv.call(kinova_msgs.srv.StopRequest())
return
# 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 execute_grasp():
# 执行抓取。
global MOVING
global CURR_Z
global start_force_srv
global stop_force_srv
# 得到位置。
msg = rospy.wait_for_message('/ggcnn/out/command',
std_msgs.msg.Float32MultiArray)
d = list(msg.data)
# 计算抓爪宽度。
grip_width = d[4]
# 将以像素为单位的宽度转换为毫米。
# 0.07是从末端执行器（CURR_Z）到摄像机的距离。
#对于实感，每个像素约0.1度。
g_width = 2 * ((CURR_Z + 0.07)) * np.tan(
0.1 * grip_width / 2.0 / 180.0 * np.pi) * 1000
# 转换为电机位置。
g = min((1 - (min(g_width, 70) / 70)) * (6800 - 4000) + 4000, 5500)
set_finger_positions([g, g])
rospy.sleep(0.5)
# 握把的姿势（仅位置）在相机框架中。
gp = geometry_msgs.msg.Pose()
gp.position.x = d[0]
gp.position.y = d[1]
gp.position.z = d[2]
gp.orientation.w = 1
# 转换为基本框架，并添加角度（以确保平面抓地力，不保证相机垂直）。
gp_base = convert_pose(gp, 'camera_depth_optical_frame',
'm1n6s200_link_base')
q = tft.quaternion_from_euler(np.pi, 0, d[3])
gp_base.orientation.x = q[0]
gp_base.orientation.y = q[1]
gp_base.orientation.z = q[2]
gp_base.orientation.w = q[3]
publish_pose_as_transform(gp_base, 'm1n6s200_link_base', 'G', 0.5)
# 初始姿势的偏移量。
initial_offset = 0.20
gp_base.position.z += initial_offset
# 禁用力控制，使机器人更加精确。
stop_force_srv.call(kinova_msgs.srv.StopRequest())
move_to_pose(gp_base)
rospy.sleep(0.1)
# 启动力控制，有助于防止不良碰撞。
start_force_srv.call(kinova_msgs.srv.StartRequest())
rospy.sleep(0.25)
# 重设位置
gp_base.position.z -= initial_offset
# 标记以检查碰撞。
MOVING = True
# 为控制器生成非线性。
cart_cov = generate_cartesian_covariance(0)
# 在速度控制下直线向下移动。
velo_pub = rospy.Publisher('/m1n6s200_driver/in/cartesian_velocity',
kinova_msgs.msg.PoseVelocity,
queue_size=1)
while MOVING and CURR_Z - 0.02 > gp_base.position.z:
dz = gp_base.position.z - CURR_Z - 0.03 # Offset by a few cm for the fingertips.
MAX_VELO_Z = 0.08
dz = max(min(dz, MAX_VELO_Z), -1.0 * MAX_VELO_Z)
v = np.array([0, 0, dz])
vc = list(np.dot(v, cart_cov)) + [0, 0, 0]
velo_pub.publish(kinova_msgs.msg.PoseVelocity(*vc))
rospy.sleep(1 / 100.0)
MOVING = False
# 闭合手指。
rospy.sleep(0.1)
set_finger_positions([8000, 8000])
rospy.sleep(0.5)
# 移回初始位置。
gp_base.position.z += initial_offset
gp_base.orientation.x = 1
gp_base.orientation.y = 0
gp_base.orientation.z = 0
gp_base.orientation.w = 0
move_to_pose(gp_base)
stop_force_srv.call(kinova_msgs.srv.StopRequest())
return
