def niryo_demo():
#Approach vector and offset distance to compensate for gripper length
approach = [0, 0, -1]
grasp_offset = -0.12
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_niryo_robot')
n = NiryoOne()
n.calibrate_auto()
n.activate_learning_mode(False)
n.change_tool(TOOL_GRIPPER_2_ID)
n.open_gripper(TOOL_GRIPPER_2_ID, 200)
print("[GRIPPER 3 OPENED]")
robot = moveit_commander.RobotCommander()
#scene = moveit_commander.PlanningSceneInterface()
group_name = "arm"
group = moveit_commander.MoveGroupCommander(group_name)
group.set_planning_time(10)
listener = tf.TransformListener()
#Let the buffer fill so we can get the frame list
time.sleep(1)
frame_list = listener.getFrameStrings()
# listener.waitForTransform("camera_color_optical_frame", "ground_link", rospy.Time(), rospy.Duration(2.0))
# (trans, quat) = listener.lookupTransform("camera_color_optical_frame", "ground_link", rospy.Time(0))
# camera_static_transform = subprocess.Popen(
# ["rosrun", "tf", "static_tranform_publisher", str(trans[0]), str(trans[1]), str(trans[2]), str(quat[0]), str(quat[1]), str(quat[2]), str(quat[3]), "100"])
# print("[INFO] Camera-robot link established, you can remove the ARUCO marker now.")
# rospy.sleep(5)
#One object only
# marker_frame = [e for e in frame_list if e[:7] == "object_"][0]
get_object_marker_position()
print("Transforming point from frame: " + marker_pose.header.frame_id)
listener.waitForTransform("base_link", marker_pose.header.frame_id,
rospy.Time(), rospy.Duration(2.0))
# (trans, quat) = listener.lookupTransform("base_link", marker_frame, rospy.Time(0))
marker_point = listener.transformPose(
"base_link", marker_pose) # the marker position in arm base frame
pprint(marker_point)
#print "Translation [x, y, z] = " + str(trans)
#print "Orientation [x, y, z, w] = " + str(quat)
q = Quaternion(marker_point.pose.orientation.w,
marker_point.pose.orientation.x,
marker_point.pose.orientation.y,
marker_point.pose.orientation.z)
z_new = q.rotate([0.0, 0.0, 1.0])
theta = math.acos(z_new[0])
qy = Quaternion(axis=[0, 1, 0], angle=math.pi / 2)
if z_new[1] > 0:
sign = -1
else:
sign = 1
qz = Quaternion(axis=[0, 0, 1], angle=sign * (math.pi / 2 - theta))
q_gripper = qz * qy
point = marker_point.pose.position
pick_target = geometry_msgs.msg.Pose()
pick_target.position.x = point.x + grasp_offset * approach[0]
pick_target.position.y = point.y + grasp_offset * approach[1]
pick_target.position.z = point.z + grasp_offset * approach[2]
#pick_target.orientation.x = 0.0
#pick_target.orientation.y = 1.0/math.sqrt(2)
#pick_target.orientation.z = 0.0
#pick_target.orientation.w = 1.0/math.sqrt(2)
pick_target.orientation.x = q_gripper[1]
pick_target.orientation.y = q_gripper[2]
pick_target.orientation.z = q_gripper[3]
pick_target.orientation.w = q_gripper[0]
group.set_pose_target(pick_target)
pprint(pick_target)
#pprint(pick_target)
plan = group.plan()
group.go(wait=True)
n.close_gripper(TOOL_GRIPPER_2_ID, 200)
print("[GRIPPER 3 CLOSED]")
place_target = geometry_msgs.msg.Pose()
place_target.position.x = pick_target.position.x
place_target.position.y = pick_target.position.y
place_target.position.z = pick_target.position.z + 0.1 #Lift the cube 10cm
place_target.orientation.x = 0.0
place_target.orientation.y = 1.0 / math.sqrt(2)
place_target.orientation.z = 0.0
place_target.orientation.w = 1.0 / math.sqrt(2)
group.set_pose_target(place_target)
plan = group.plan()
group.go(wait=True)
#Place cube in the -y position of starting position
place_target = geometry_msgs.msg.Pose()
place_target.position.x = pick_target.position.x
place_target.position.y = -pick_target.position.y
place_target.position.z = pick_target.position.z
place_target.orientation.x = 0.0
place_target.orientation.y = 1.0 / math.sqrt(2)
place_target.orientation.z = 0.0
place_target.orientation.w = 1.0 / math.sqrt(2)
group.set_pose_target(place_target)
plan = group.plan()
group.go(wait=True)
n.open_gripper(TOOL_GRIPPER_2_ID, 200)
print("[GRIPPER 3 OPENED]")
#Move to resting position
resting_joints = [0, 0.64, -1.39, 0, 0, 0]
n.move_joints(resting_joints)
n.close_gripper(TOOL_GRIPPER_2_ID, 200)
print("[GRIPPER 3 CLOSED]")
n.activate_learning_mode(True)
moveit_commander.roscpp_shutdown()
